From 1803c13aeb7591ea75a49eead0be58cd4ff24374 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Wed, 4 Oct 2023 10:08:55 -0500 Subject: [PATCH] IK Solver Update (#30) * Migrate utility function to reach * Replace deprecated function * Check that IK solver has been loaded for move group * Run format jobs on 20.04 * Ran clang format --- .github/workflows/clang_format.yml | 2 +- .github/workflows/cmake_format.yml | 2 +- dependencies.repos | 2 +- include/reach_ros/utils.h | 4 ++-- src/evaluation/distance_penalty_moveit.cpp | 3 ++- src/evaluation/joint_penalty_moveit.cpp | 3 ++- src/evaluation/manipulability_moveit.cpp | 3 ++- src/ik/moveit_ik_solver.cpp | 6 +++++- src/python/python_bindings.cpp | 1 - src/utils.cpp | 18 ++---------------- 10 files changed, 18 insertions(+), 26 deletions(-) diff --git a/.github/workflows/clang_format.yml b/.github/workflows/clang_format.yml index fed4bb6..1b9ed2f 100644 --- a/.github/workflows/clang_format.yml +++ b/.github/workflows/clang_format.yml @@ -13,7 +13,7 @@ on: jobs: clang_format: name: Clang-Format - runs-on: ubuntu-latest + runs-on: ubuntu-20.04 steps: - uses: actions/checkout@v1 diff --git a/.github/workflows/cmake_format.yml b/.github/workflows/cmake_format.yml index d58ae0a..d8780c7 100644 --- a/.github/workflows/cmake_format.yml +++ b/.github/workflows/cmake_format.yml @@ -13,7 +13,7 @@ on: jobs: cmake_format: name: CMake-Format - runs-on: ubuntu-latest + runs-on: ubuntu-20.04 steps: - uses: actions/checkout@v1 diff --git a/dependencies.repos b/dependencies.repos index f194a9b..38644ac 100644 --- a/dependencies.repos +++ b/dependencies.repos @@ -9,4 +9,4 @@ - git: local-name: reach uri: https://github.com/ros-industrial/reach.git - version: 1.5.2 + version: 1.6.0 diff --git a/include/reach_ros/utils.h b/include/reach_ros/utils.h index 625d9f8..fc74500 100644 --- a/include/reach_ros/utils.h +++ b/include/reach_ros/utils.h @@ -49,8 +49,8 @@ visualization_msgs::msg::InteractiveMarker makeInteractiveMarker(const std::stri visualization_msgs::msg::Marker makeMarker(const std::vector& pts, const std::string& frame, const double scale, const std::string& ns = ""); -std::vector transcribeInputMap(const std::map& input, - const std::vector& joint_names); +[[deprecated]] std::vector transcribeInputMap(const std::map& input, + const std::vector& joint_names); /** * @brief Returns a singleton ROS2 node for accessing parameters and publishing data diff --git a/src/evaluation/distance_penalty_moveit.cpp b/src/evaluation/distance_penalty_moveit.cpp index 8eebeb4..3c92668 100644 --- a/src/evaluation/distance_penalty_moveit.cpp +++ b/src/evaluation/distance_penalty_moveit.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include namespace reach_ros @@ -58,7 +59,7 @@ void DistancePenaltyMoveIt::setTouchLinks(const std::vector& touch_ double DistancePenaltyMoveIt::calculateScore(const std::map& pose) const { // Pull the joints from the planning group out of the input pose map - std::vector pose_subset = utils::transcribeInputMap(pose, jmg_->getActiveJointModelNames()); + std::vector pose_subset = reach::extractSubset(pose, jmg_->getActiveJointModelNames()); moveit::core::RobotState state(model_); state.setJointGroupPositions(jmg_, pose_subset); state.update(); diff --git a/src/evaluation/joint_penalty_moveit.cpp b/src/evaluation/joint_penalty_moveit.cpp index cc54d03..de4667e 100644 --- a/src/evaluation/joint_penalty_moveit.cpp +++ b/src/evaluation/joint_penalty_moveit.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include namespace reach_ros @@ -37,7 +38,7 @@ JointPenaltyMoveIt::JointPenaltyMoveIt(moveit::core::RobotModelConstPtr model, c double JointPenaltyMoveIt::calculateScore(const std::map& pose) const { // Pull the joints from the planning group out of the input pose map - std::vector pose_subset = utils::transcribeInputMap(pose, jmg_->getActiveJointModelNames()); + std::vector pose_subset = reach::extractSubset(pose, jmg_->getActiveJointModelNames()); Eigen::Map min(joints_min_.data(), joints_min_.size()); Eigen::Map max(joints_max_.data(), joints_max_.size()); Eigen::Map joints(pose_subset.data(), pose_subset.size()); diff --git a/src/evaluation/manipulability_moveit.cpp b/src/evaluation/manipulability_moveit.cpp index 59ea6f4..d4b55d2 100644 --- a/src/evaluation/manipulability_moveit.cpp +++ b/src/evaluation/manipulability_moveit.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include static std::vector getJacobianRowSubset(const YAML::Node& config, const std::string& key = "jacobian_row_" @@ -118,7 +119,7 @@ double ManipulabilityMoveIt::calculateScore(const std::map& moveit::core::RobotState state(model_); // Take the subset of joints in the joint model group out of the input pose - std::vector pose_subset = utils::transcribeInputMap(pose, jmg_->getActiveJointModelNames()); + std::vector pose_subset = reach::extractSubset(pose, jmg_->getActiveJointModelNames()); state.setJointGroupPositions(jmg_, pose_subset); state.update(); diff --git a/src/ik/moveit_ik_solver.cpp b/src/ik/moveit_ik_solver.cpp index b4f98e6..4e2d819 100644 --- a/src/ik/moveit_ik_solver.cpp +++ b/src/ik/moveit_ik_solver.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include namespace @@ -45,6 +46,9 @@ MoveItIKSolver::MoveItIKSolver(moveit::core::RobotModelConstPtr model, const std { if (!jmg_) throw std::runtime_error("Failed to initialize joint model group for planning group '" + planning_group + "'"); + if (!jmg_->getSolverInstance()) + throw std::runtime_error("No kinematics solver instantiated for planning group '" + planning_group + + "'. Check that the 'kinematics.yaml' file was loaded as a parameter"); scene_.reset(new planning_scene::PlanningScene(model_)); @@ -62,7 +66,7 @@ std::vector> MoveItIKSolver::solveIK(const Eigen::Isometry3d const std::vector& joint_names = jmg_->getActiveJointModelNames(); - std::vector seed_subset = utils::transcribeInputMap(seed, joint_names); + std::vector seed_subset = reach::extractSubset(seed, joint_names); state.setJointGroupPositions(jmg_, seed_subset); state.update(); diff --git a/src/python/python_bindings.cpp b/src/python/python_bindings.cpp index 56e56fa..41fd5ab 100644 --- a/src/python/python_bindings.cpp +++ b/src/python/python_bindings.cpp @@ -30,7 +30,6 @@ namespace bp = boost::python; namespace reach_ros { - void init_ros(const bp::list& argv) { int argc = bp::len(argv); diff --git a/src/utils.cpp b/src/utils.cpp index 9ceec4f..d61be8a 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -19,6 +19,7 @@ #include #include #include +#include const static double ARROW_SCALE_RATIO = 6.0; const static double NEIGHBOR_MARKER_SCALE_RATIO = ARROW_SCALE_RATIO / 2.0; @@ -217,22 +218,7 @@ visualization_msgs::msg::Marker makeMarker(const std::vector transcribeInputMap(const std::map& input, const std::vector& joint_names) { - if (joint_names.size() > input.size()) - throw std::runtime_error("Seed pose size was not at least as large as the number of joints in the planning group"); - - // Pull the joints of the planning group out of the input map - std::vector joints; - joints.reserve(joint_names.size()); - for (const std::string& name : joint_names) - { - const auto it = input.find(name); - if (it == input.end()) - throw std::runtime_error("Joint '" + name + "' in the planning group was not in the input map"); - - joints.push_back(it->second); - } - - return joints; + return reach::extractSubset(input, joint_names); } } // namespace utils