From c873ba5a9e764f0f746d79cf73676cc2b48053fd Mon Sep 17 00:00:00 2001 From: Kzernobog Date: Wed, 18 Sep 2024 15:15:54 +0100 Subject: [PATCH 1/9] 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")); From f15c196cf283ca1f888e3556f9a72b1474cd3c8a Mon Sep 17 00:00:00 2001 From: Kzernobog Date: Wed, 18 Sep 2024 16:10:57 +0100 Subject: [PATCH 2/9] adds a small introduction on the readme --- README.md | 37 +++++++++++----------------------- src/ktopt_planning_context.cpp | 3 --- 2 files changed, 12 insertions(+), 28 deletions(-) diff --git a/README.md b/README.md index cfd88e0..eb56c56 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,16 @@ # Experimental MoveIt 2 - Drake Integration -Under construction +`moveit_drake` brings together the vertical ROS integration of the +[MoveIt2](https://moveit.ai/) motion planning framework, with the Mathematical +Programming interface within [drake](https://drake.mit.edu/). This allows the +user to setup motion planning as an optimization problem within ROS, with the +rich specification of constraints and costs provided by `drake`. ## Docker Workflow (Preferred and tested) ### Requirements -`docker` and `docker-compose` - Follow instructions [here](https://docs.docker.com/engine/install/ubuntu/). +`docker` and `docker-compose` - Follow instructions +[here](https://docs.docker.com/engine/install/ubuntu/). ### Steps The following steps clone and build the base image that you will require to @@ -33,7 +38,9 @@ Follow [instructions](#build-moveit_drake) below to build `moveit_drake` ### Build `moveit_drake` -Follow the [MoveIt Source Build](https://moveit.ros.org/install-moveit2/source/) instructions to set up a colcon workspace with MoveIt from source. +Follow the [MoveIt Source +Build](https://moveit.ros.org/install-moveit2/source/) instructions to set up a +colcon workspace with MoveIt from source. Open a command line to your colcon workspace: @@ -58,28 +65,8 @@ ros2 launch moveit_drake pipeline_testbench.launch.py ### Development -- Use [pre-commit to format your code](https://moveit.ros.org/documentation/contributing/code/#pre-commit-formatting-checks) - -# Todo section - -This section keeps a list of immediate todos, will be deleted before repo release - -- [x] Create drake planning pipeline option in `pipeline_testbench.launch.py` -- [x] Declare to moveit, to use the drake ktopt planning pipeline -- [ ] Build planner manager and planning context to display info from `moveit` - and `drake` instance. - - [ ] Generated placeholder classes mimicking `stomp` implementation. - - [ ] Display info messages during testbench runtime. - - [ ] -- [ ] read Robot description and display onto drake visualizer - -### Doubts -- [x] stomp_moveit::ParamListener, where is this being declared -- [ ] Why is the parameter file in the "res" directory - -### Potential issues -- Assumes that planner managers initialize will set robot description before a - call to getPlanningContext. +- Use [pre-commit to format your + code](https://moveit.ros.org/documentation/contributing/code/#pre-commit-formatting-checks) ### Some helper commands To just rebuild `moveit_drake` diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index 1fee418..836ce24 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -181,9 +181,6 @@ void KTOptPlanningContext::setRobotDescription(std::string robot_description) // TODO:(kamiradi) Figure out object parsing // 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 = params_.drake_robot_description.c_str(); const std::string urdf = PackageMap{}.ResolveUrl(ModelUrl); auto robot_instance = Parser(&plant, &scene_graph).AddModels(urdf); From ec9acd3171d7bcf4c1adfd58a1d77a439b9dff49 Mon Sep 17 00:00:00 2001 From: Kzernobog Date: Thu, 19 Sep 2024 17:18:33 +0100 Subject: [PATCH 3/9] [FT] Sets up pos/vel bounds in KTOPT This commit adds upper/lower position/velocity bounds. Reads them from the robot model VariableBound API and sets them up as Drake PositionBund and VelocityBound constraints. Adds some additional docs on readme --- README.md | 8 +++++ src/ktopt_planning_context.cpp | 60 ++++++++++++++++++++++++++++++---- 2 files changed, 62 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index eb56c56..ee6f96f 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,19 @@ # Experimental MoveIt 2 - Drake Integration +NOTE: Experimental and will continue to have breaking changes until first +release. + `moveit_drake` brings together the vertical ROS integration of the [MoveIt2](https://moveit.ai/) motion planning framework, with the Mathematical Programming interface within [drake](https://drake.mit.edu/). This allows the user to setup motion planning as an optimization problem within ROS, with the rich specification of constraints and costs provided by `drake`. +## Features + +- Exposes `KinematicTrajectoryOptimization` implementation in `drake`. +- Exposes `TOPPRA` implementation in `drake`. + ## Docker Workflow (Preferred and tested) ### Requirements diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index 836ce24..51e17f2 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -48,6 +48,54 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) const auto joint_model_group = getPlanningScene()->getRobotModel()->getJointModelGroup(getGroupName()); RCLCPP_INFO_STREAM(getLogger(), "Planning for group: " << getGroupName()); + // extract position and velocity bounds + std::vector lower_position_bounds; + std::vector upper_position_bounds; + std::vector lower_velocity_bounds; + std::vector upper_velocity_bounds; + for (const auto &joint_model : joint_model_group->getActiveJointModels()) { + const std::string &joint_name = joint_model->getName(); + const moveit::core::VariableBounds &bounds = + joint_model->getVariableBounds()[0]; + + lower_position_bounds.push_back(bounds.min_position_); + upper_position_bounds.push_back(bounds.max_position_); + lower_velocity_bounds.push_back(-bounds.max_velocity_); + upper_velocity_bounds.push_back(bounds.max_velocity_); + + // std::cout << "Joint " << joint_name << ": Position [" + // << bounds.min_position_ << ", " << bounds.max_position_ + // << "], Velocity [-" << bounds.max_velocity_ << ", " + // << bounds.max_velocity_ << "]" << std::endl; + } + int num_positions = plant.num_positions(); + int num_velocities = plant.num_velocities(); + + // Ensure the bounds have the correct size + if (lower_position_bounds.size() != num_positions || + upper_position_bounds.size() != num_positions) { + lower_position_bounds.resize(num_positions, + -1e6); + upper_position_bounds.resize(num_positions, + 1e6); + } + if (lower_velocity_bounds.size() != num_velocities || + upper_velocity_bounds.size() != num_velocities) { + // Resize velocity bounds to match number of velocities, if necessary + lower_velocity_bounds.resize(num_velocities, + -1e6); + upper_velocity_bounds.resize(num_velocities, + 1e6); + } + Eigen::Map lower_position_bounds_eigen( + lower_position_bounds.data(), lower_position_bounds.size()); + Eigen::Map upper_position_bounds_eigen( + upper_position_bounds.data(), upper_position_bounds.size()); + Eigen::Map lower_velocity_bounds_eigen( + lower_velocity_bounds.data(), lower_velocity_bounds.size()); + Eigen::Map upper_velocity_bounds_eigen( + upper_velocity_bounds.data(), upper_velocity_bounds.size()); + // q represents the complete state (joint positions and velocities) VectorXd q = VectorXd::Zero(plant.num_positions() + plant.num_velocities()); q << moveit::drake::getJointPositionVector(start_state, getGroupName(), plant); @@ -97,12 +145,12 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) trajopt.AddPathVelocityConstraint(goal_velocity, goal_velocity, 1.0); // TODO: Add constraints on joint position/velocity/acceleration - // trajopt.AddPositionBounds( - // plant_->GetPositionLowerLimits(), - // plant_->GetPositionUpperLimits()); - // trajopt.AddVelocityBounds( - // plant_->GetVelocityLowerLimits(), - // plant_->GetVelocityUpperLimits()); + trajopt.AddPositionBounds( + lower_position_bounds_eigen, + upper_position_bounds_eigen); + trajopt.AddVelocityBounds( + lower_velocity_bounds_eigen, + upper_velocity_bounds_eigen); // Add constraints on duration // TODO: These should be parameters From 9c59be2cf7cc2b5ccfb575f5df4e3b2cdbc185c0 Mon Sep 17 00:00:00 2001 From: Kzernobog Date: Thu, 19 Sep 2024 17:31:05 +0100 Subject: [PATCH 4/9] pre-commits --- src/ktopt_planning_context.cpp | 54 +++++++++++++++------------------- 1 file changed, 23 insertions(+), 31 deletions(-) diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index 51e17f2..e35518b 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -53,10 +53,10 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) std::vector upper_position_bounds; std::vector lower_velocity_bounds; std::vector upper_velocity_bounds; - for (const auto &joint_model : joint_model_group->getActiveJointModels()) { - const std::string &joint_name = joint_model->getName(); - const moveit::core::VariableBounds &bounds = - joint_model->getVariableBounds()[0]; + for (const auto& joint_model : joint_model_group->getActiveJointModels()) + { + const std::string& joint_name = joint_model->getName(); + const moveit::core::VariableBounds& bounds = joint_model->getVariableBounds()[0]; lower_position_bounds.push_back(bounds.min_position_); upper_position_bounds.push_back(bounds.max_position_); @@ -69,32 +69,28 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) // << bounds.max_velocity_ << "]" << std::endl; } int num_positions = plant.num_positions(); - int num_velocities = plant.num_velocities(); + int num_velocities = plant.num_velocities(); // Ensure the bounds have the correct size - if (lower_position_bounds.size() != num_positions || - upper_position_bounds.size() != num_positions) { - lower_position_bounds.resize(num_positions, - -1e6); - upper_position_bounds.resize(num_positions, - 1e6); + if (lower_position_bounds.size() != num_positions || upper_position_bounds.size() != num_positions) + { + lower_position_bounds.resize(num_positions, -1e6); + upper_position_bounds.resize(num_positions, 1e6); } - if (lower_velocity_bounds.size() != num_velocities || - upper_velocity_bounds.size() != num_velocities) { + if (lower_velocity_bounds.size() != num_velocities || upper_velocity_bounds.size() != num_velocities) + { // Resize velocity bounds to match number of velocities, if necessary - lower_velocity_bounds.resize(num_velocities, - -1e6); - upper_velocity_bounds.resize(num_velocities, - 1e6); + lower_velocity_bounds.resize(num_velocities, -1e6); + upper_velocity_bounds.resize(num_velocities, 1e6); } - Eigen::Map lower_position_bounds_eigen( - lower_position_bounds.data(), lower_position_bounds.size()); - Eigen::Map upper_position_bounds_eigen( - upper_position_bounds.data(), upper_position_bounds.size()); - Eigen::Map lower_velocity_bounds_eigen( - lower_velocity_bounds.data(), lower_velocity_bounds.size()); - Eigen::Map upper_velocity_bounds_eigen( - upper_velocity_bounds.data(), upper_velocity_bounds.size()); + Eigen::Map lower_position_bounds_eigen(lower_position_bounds.data(), + lower_position_bounds.size()); + Eigen::Map upper_position_bounds_eigen(upper_position_bounds.data(), + upper_position_bounds.size()); + Eigen::Map lower_velocity_bounds_eigen(lower_velocity_bounds.data(), + lower_velocity_bounds.size()); + Eigen::Map upper_velocity_bounds_eigen(upper_velocity_bounds.data(), + upper_velocity_bounds.size()); // q represents the complete state (joint positions and velocities) VectorXd q = VectorXd::Zero(plant.num_positions() + plant.num_velocities()); @@ -145,12 +141,8 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) trajopt.AddPathVelocityConstraint(goal_velocity, goal_velocity, 1.0); // TODO: Add constraints on joint position/velocity/acceleration - trajopt.AddPositionBounds( - lower_position_bounds_eigen, - upper_position_bounds_eigen); - trajopt.AddVelocityBounds( - lower_velocity_bounds_eigen, - upper_velocity_bounds_eigen); + trajopt.AddPositionBounds(lower_position_bounds_eigen, upper_position_bounds_eigen); + trajopt.AddVelocityBounds(lower_velocity_bounds_eigen, upper_velocity_bounds_eigen); // Add constraints on duration // TODO: These should be parameters From f2202db429cfa827b66dd968e024a1cf8441a2d4 Mon Sep 17 00:00:00 2001 From: Kzernobog Date: Fri, 20 Sep 2024 15:49:29 +0100 Subject: [PATCH 5/9] addresses review comments --- README.md | 6 ++++-- res/ktopt_moveit_parameters.yaml | 3 --- src/ktopt_planning_context.cpp | 8 ++------ 3 files changed, 6 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index ee6f96f..816521f 100644 --- a/README.md +++ b/README.md @@ -11,8 +11,10 @@ rich specification of constraints and costs provided by `drake`. ## Features -- Exposes `KinematicTrajectoryOptimization` implementation in `drake`. -- Exposes `TOPPRA` implementation in `drake`. +- Exposes + [`KinematicTrajectoryOptimization`](https://drake.mit.edu/doxygen_cxx/classdrake_1_1planning_1_1trajectory__optimization_1_1_kinematic_trajectory_optimization.html) + implementation in `drake`. +- Exposes [`TOPPRA`](https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_toppra.html) implementation in `drake`. ## Docker Workflow (Preferred and tested) diff --git a/res/ktopt_moveit_parameters.yaml b/res/ktopt_moveit_parameters.yaml index b11c8b0..2bd661e 100644 --- a/res/ktopt_moveit_parameters.yaml +++ b/res/ktopt_moveit_parameters.yaml @@ -3,9 +3,6 @@ ktopt_interface: 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, diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index e35518b..cbf6e27 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -63,13 +63,9 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) lower_velocity_bounds.push_back(-bounds.max_velocity_); upper_velocity_bounds.push_back(bounds.max_velocity_); - // std::cout << "Joint " << joint_name << ": Position [" - // << bounds.min_position_ << ", " << bounds.max_position_ - // << "], Velocity [-" << bounds.max_velocity_ << ", " - // << bounds.max_velocity_ << "]" << std::endl; } - int num_positions = plant.num_positions(); - int num_velocities = plant.num_velocities(); + const int num_positions = plant.num_positions(); + const int num_velocities = plant.num_velocities(); // Ensure the bounds have the correct size if (lower_position_bounds.size() != num_positions || upper_position_bounds.size() != num_positions) From 61e43817fbe061445184b583cfe250f47b5e1893 Mon Sep 17 00:00:00 2001 From: Kzernobog Date: Fri, 20 Sep 2024 15:51:40 +0100 Subject: [PATCH 6/9] pre-commit stuff --- src/ktopt_planning_context.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index cbf6e27..123c107 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -62,7 +62,6 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) upper_position_bounds.push_back(bounds.max_position_); lower_velocity_bounds.push_back(-bounds.max_velocity_); upper_velocity_bounds.push_back(bounds.max_velocity_); - } const int num_positions = plant.num_positions(); const int num_velocities = plant.num_velocities(); From e5ab7f4b1c51cd52b77e1561d2562f37b2bc4022 Mon Sep 17 00:00:00 2001 From: Kzernobog Date: Fri, 4 Oct 2024 09:13:47 +0100 Subject: [PATCH 7/9] adds acceleration and place holder jerk constraint --- res/ktopt_moveit_parameters.yaml | 8 ++++++++ src/ktopt_planning_context.cpp | 19 +++++++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/res/ktopt_moveit_parameters.yaml b/res/ktopt_moveit_parameters.yaml index 2bd661e..4a993c0 100644 --- a/res/ktopt_moveit_parameters.yaml +++ b/res/ktopt_moveit_parameters.yaml @@ -44,3 +44,11 @@ ktopt_interface: gt_eq<>: [0.0] } } + joint_jerk_bound: { + type: double, + description: "Maximum jerk that is allowed for generated trajectory", + default_value: 1.0, + validation: { + gt_eq<>: [0.0] + } + } diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index 123c107..aa2122b 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -53,6 +53,8 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) std::vector upper_position_bounds; std::vector lower_velocity_bounds; std::vector upper_velocity_bounds; + std::vector lower_acceleration_bounds; + std::vector upper_acceleration_bounds; for (const auto& joint_model : joint_model_group->getActiveJointModels()) { const std::string& joint_name = joint_model->getName(); @@ -62,6 +64,8 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) upper_position_bounds.push_back(bounds.max_position_); lower_velocity_bounds.push_back(-bounds.max_velocity_); upper_velocity_bounds.push_back(bounds.max_velocity_); + lower_acceleration_bounds.push_back(-bounds.max_acceleration_); + upper_acceleration_bounds.push_back(bounds.max_acceleration_); } const int num_positions = plant.num_positions(); const int num_velocities = plant.num_velocities(); @@ -78,6 +82,12 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) lower_velocity_bounds.resize(num_velocities, -1e6); upper_velocity_bounds.resize(num_velocities, 1e6); } + if (lower_acceleration_bounds.size() != num_velocities || upper_acceleration_bounds.size() != num_velocities) + { + // Resize velocity bounds to match number of velocities, if necessary + lower_acceleration_bounds.resize(num_velocities, -1e6); + upper_acceleration_bounds.resize(num_velocities, 1e6); + } Eigen::Map lower_position_bounds_eigen(lower_position_bounds.data(), lower_position_bounds.size()); Eigen::Map upper_position_bounds_eigen(upper_position_bounds.data(), @@ -86,6 +96,13 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) lower_velocity_bounds.size()); Eigen::Map upper_velocity_bounds_eigen(upper_velocity_bounds.data(), upper_velocity_bounds.size()); + Eigen::Map lower_acceleration_bounds_eigen(lower_acceleration_bounds.data(), + lower_acceleration_bounds.size()); + Eigen::Map upper_acceleration_bounds_eigen(upper_acceleration_bounds.data(), + upper_acceleration_bounds.size()); + + Eigen::VectorXd lower_jerk_bounds_eigen = Eigen::VectorXd::Constant(num_velocities, -1 * params_.joint_jerk_bound); + Eigen::VectorXd upper_jerk_bounds_eigen = Eigen::VectorXd::Constant(num_velocities, params_.joint_jerk_bound); // q represents the complete state (joint positions and velocities) VectorXd q = VectorXd::Zero(plant.num_positions() + plant.num_velocities()); @@ -138,6 +155,8 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) // TODO: Add constraints on joint position/velocity/acceleration trajopt.AddPositionBounds(lower_position_bounds_eigen, upper_position_bounds_eigen); trajopt.AddVelocityBounds(lower_velocity_bounds_eigen, upper_velocity_bounds_eigen); + trajopt.AddAccelerationBounds(lower_acceleration_bounds_eigen, upper_acceleration_bounds_eigen); + trajopt.AddJerkBounds(lower_jerk_bounds_eigen, upper_jerk_bounds_eigen); // Add constraints on duration // TODO: These should be parameters From 75d73ed8c67e9e6661619152b04591bd895033d9 Mon Sep 17 00:00:00 2001 From: Kzernobog Date: Fri, 4 Oct 2024 15:44:38 +0100 Subject: [PATCH 8/9] initialises bounds with dofs by reserving memory - --- README.md | 7 +++++-- src/ktopt_planning_context.cpp | 30 ++++++++++++++++++++---------- 2 files changed, 25 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index 816521f..0f6574f 100644 --- a/README.md +++ b/README.md @@ -4,8 +4,8 @@ NOTE: Experimental and will continue to have breaking changes until first release. `moveit_drake` brings together the vertical ROS integration of the -[MoveIt2](https://moveit.ai/) motion planning framework, with the Mathematical -Programming interface within [drake](https://drake.mit.edu/). This allows the +[MoveIt 2](https://moveit.ai/) motion planning framework, with the Mathematical +Programming interface within [Drake](https://drake.mit.edu/). This allows the user to setup motion planning as an optimization problem within ROS, with the rich specification of constraints and costs provided by `drake`. @@ -78,6 +78,9 @@ 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) + # inside the moveit_drake package + pre-commit run -a + ### Some helper commands To just rebuild `moveit_drake` ``` diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index aa2122b..9225801 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -16,6 +16,9 @@ rclcpp::Logger getLogger() { return moveit::getLogger("moveit.planners.ktopt_interface.planning_context"); } + +constexpr double kDefaultJointMaxPosition = 1.0e6; + } // namespace KTOptPlanningContext::KTOptPlanningContext(const std::string& name, const std::string& group_name, @@ -47,6 +50,8 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) const moveit::core::RobotState start_state(*getPlanningScene()->getCurrentStateUpdated(req.start_state)); const auto joint_model_group = getPlanningScene()->getRobotModel()->getJointModelGroup(getGroupName()); RCLCPP_INFO_STREAM(getLogger(), "Planning for group: " << getGroupName()); + const int num_positions = plant.num_positions(); + const int num_velocities = plant.num_velocities(); // extract position and velocity bounds std::vector lower_position_bounds; @@ -55,10 +60,17 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) std::vector upper_velocity_bounds; std::vector lower_acceleration_bounds; std::vector upper_acceleration_bounds; + lower_position_bounds.reserve(num_positions); + upper_position_bounds.reserve(num_positions); + lower_velocity_bounds.reserve(num_positions); + upper_velocity_bounds.reserve(num_positions); + lower_acceleration_bounds.reserve(num_positions); + upper_acceleration_bounds.reserve(num_positions); + for (const auto& joint_model : joint_model_group->getActiveJointModels()) { - const std::string& joint_name = joint_model->getName(); - const moveit::core::VariableBounds& bounds = joint_model->getVariableBounds()[0]; + const auto& joint_name = joint_model->getName(); + const auto& bounds = joint_model->getVariableBounds()[0]; lower_position_bounds.push_back(bounds.min_position_); upper_position_bounds.push_back(bounds.max_position_); @@ -67,26 +79,24 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) lower_acceleration_bounds.push_back(-bounds.max_acceleration_); upper_acceleration_bounds.push_back(bounds.max_acceleration_); } - const int num_positions = plant.num_positions(); - const int num_velocities = plant.num_velocities(); // Ensure the bounds have the correct size if (lower_position_bounds.size() != num_positions || upper_position_bounds.size() != num_positions) { - lower_position_bounds.resize(num_positions, -1e6); - upper_position_bounds.resize(num_positions, 1e6); + lower_position_bounds.resize(num_positions, -kDefaultJointMaxPosition); + upper_position_bounds.resize(num_positions, kDefaultJointMaxPosition); } if (lower_velocity_bounds.size() != num_velocities || upper_velocity_bounds.size() != num_velocities) { // Resize velocity bounds to match number of velocities, if necessary - lower_velocity_bounds.resize(num_velocities, -1e6); - upper_velocity_bounds.resize(num_velocities, 1e6); + lower_velocity_bounds.resize(num_velocities, -kDefaultJointMaxPosition); + upper_velocity_bounds.resize(num_velocities, kDefaultJointMaxPosition); } if (lower_acceleration_bounds.size() != num_velocities || upper_acceleration_bounds.size() != num_velocities) { // Resize velocity bounds to match number of velocities, if necessary - lower_acceleration_bounds.resize(num_velocities, -1e6); - upper_acceleration_bounds.resize(num_velocities, 1e6); + lower_acceleration_bounds.resize(num_velocities, -kDefaultJointMaxPosition); + upper_acceleration_bounds.resize(num_velocities, kDefaultJointMaxPosition); } Eigen::Map lower_position_bounds_eigen(lower_position_bounds.data(), lower_position_bounds.size()); From a88ce75fe615d4de3699bf5c640b5d35b5af68dc Mon Sep 17 00:00:00 2001 From: Kzernobog Date: Fri, 4 Oct 2024 15:53:44 +0100 Subject: [PATCH 9/9] [FIX] fixes incorrect integration of inactive kinematic limits --- src/ktopt_planning_context.cpp | 65 ++++++++++++++++++---------------- 1 file changed, 35 insertions(+), 30 deletions(-) diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index 9225801..1a5e4b2 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -60,44 +60,50 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) std::vector upper_velocity_bounds; std::vector lower_acceleration_bounds; std::vector upper_acceleration_bounds; + std::vector lower_jerk_bounds; + std::vector upper_jerk_bounds; lower_position_bounds.reserve(num_positions); upper_position_bounds.reserve(num_positions); lower_velocity_bounds.reserve(num_positions); upper_velocity_bounds.reserve(num_positions); lower_acceleration_bounds.reserve(num_positions); upper_acceleration_bounds.reserve(num_positions); + lower_jerk_bounds.reserve(num_positions); + upper_jerk_bounds.reserve(num_positions); - for (const auto& joint_model : joint_model_group->getActiveJointModels()) + const auto& all_joint_models = joint_model_group->getJointModels(); + for (const auto& joint_model : all_joint_models) { const auto& joint_name = joint_model->getName(); - const auto& bounds = joint_model->getVariableBounds()[0]; - - lower_position_bounds.push_back(bounds.min_position_); - upper_position_bounds.push_back(bounds.max_position_); - lower_velocity_bounds.push_back(-bounds.max_velocity_); - upper_velocity_bounds.push_back(bounds.max_velocity_); - lower_acceleration_bounds.push_back(-bounds.max_acceleration_); - upper_acceleration_bounds.push_back(bounds.max_acceleration_); - } + const bool is_active = joint_model_group->hasJointModel(joint_name); - // Ensure the bounds have the correct size - if (lower_position_bounds.size() != num_positions || upper_position_bounds.size() != num_positions) - { - lower_position_bounds.resize(num_positions, -kDefaultJointMaxPosition); - upper_position_bounds.resize(num_positions, kDefaultJointMaxPosition); - } - if (lower_velocity_bounds.size() != num_velocities || upper_velocity_bounds.size() != num_velocities) - { - // Resize velocity bounds to match number of velocities, if necessary - lower_velocity_bounds.resize(num_velocities, -kDefaultJointMaxPosition); - upper_velocity_bounds.resize(num_velocities, kDefaultJointMaxPosition); - } - if (lower_acceleration_bounds.size() != num_velocities || upper_acceleration_bounds.size() != num_velocities) - { - // Resize velocity bounds to match number of velocities, if necessary - lower_acceleration_bounds.resize(num_velocities, -kDefaultJointMaxPosition); - upper_acceleration_bounds.resize(num_velocities, kDefaultJointMaxPosition); + if (is_active) + { + // Set bounds for active joints + const auto& bounds = joint_model->getVariableBounds()[0]; + lower_position_bounds.push_back(bounds.min_position_); + upper_position_bounds.push_back(bounds.max_position_); + lower_velocity_bounds.push_back(-bounds.max_velocity_); + upper_velocity_bounds.push_back(bounds.max_velocity_); + lower_acceleration_bounds.push_back(-bounds.max_acceleration_); + upper_acceleration_bounds.push_back(bounds.max_acceleration_); + lower_jerk_bounds.push_back(-params_.joint_jerk_bound); + upper_jerk_bounds.push_back(params_.joint_jerk_bound); + } + else + { + // Set default values for inactive joints + lower_position_bounds.push_back(-kDefaultJointMaxPosition); + upper_position_bounds.push_back(kDefaultJointMaxPosition); + lower_velocity_bounds.push_back(0.0); + upper_velocity_bounds.push_back(0.0); + lower_acceleration_bounds.push_back(0.0); + upper_acceleration_bounds.push_back(0.0); + lower_jerk_bounds.push_back(0.0); + upper_jerk_bounds.push_back(0.0); + } } + Eigen::Map lower_position_bounds_eigen(lower_position_bounds.data(), lower_position_bounds.size()); Eigen::Map upper_position_bounds_eigen(upper_position_bounds.data(), @@ -110,9 +116,8 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) lower_acceleration_bounds.size()); Eigen::Map upper_acceleration_bounds_eigen(upper_acceleration_bounds.data(), upper_acceleration_bounds.size()); - - Eigen::VectorXd lower_jerk_bounds_eigen = Eigen::VectorXd::Constant(num_velocities, -1 * params_.joint_jerk_bound); - Eigen::VectorXd upper_jerk_bounds_eigen = Eigen::VectorXd::Constant(num_velocities, params_.joint_jerk_bound); + Eigen::Map lower_jerk_bounds_eigen(lower_jerk_bounds.data(), lower_jerk_bounds.size()); + Eigen::Map upper_jerk_bounds_eigen(upper_jerk_bounds.data(), upper_jerk_bounds.size()); // q represents the complete state (joint positions and velocities) VectorXd q = VectorXd::Zero(plant.num_positions() + plant.num_velocities());