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

@@ -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
 

.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
 

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

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<geometry_msgs::msg::Point>& pts, const std::string& frame,
  const double scale, const std::string& ns = "");
 
-std::vector<double> transcribeInputMap(const std::map<std::string, double>& input,
- const std::vector<std::string>& joint_names);
+[[deprecated]] std::vector<double> transcribeInputMap(const std::map<std::string, double>& input,
+  const std::vector<std::string>& joint_names);
 
 /**
  * @brief Returns a singleton ROS2 node for accessing parameters and publishing data

src/evaluation/distance_penalty_moveit.cpp

@@ -19,6 +19,7 @@
 #include <moveit/common_planning_interface_objects/common_objects.h>
 #include <moveit/planning_scene/planning_scene.h>
 #include <reach/plugin_utils.h>
+#include <reach/utils.h>
 #include <yaml-cpp/yaml.h>
 
 namespace reach_ros
@@ -58,7 +59,7 @@ void DistancePenaltyMoveIt::setTouchLinks(const std::vector<std::string>& touch_
 double DistancePenaltyMoveIt::calculateScore(const std::map<std::string, double>& pose) const
 {
  // Pull the joints from the planning group out of the input pose map
- std::vector<double> pose_subset = utils::transcribeInputMap(pose, jmg_->getActiveJointModelNames());
+ std::vector<double> pose_subset = reach::extractSubset(pose, jmg_->getActiveJointModelNames());
  moveit::core::RobotState state(model_);
  state.setJointGroupPositions(jmg_, pose_subset);
  state.update();

src/evaluation/joint_penalty_moveit.cpp

@@ -19,6 +19,7 @@
 #include <moveit/robot_model/joint_model_group.h>
 #include <moveit/common_planning_interface_objects/common_objects.h>
 #include <reach/plugin_utils.h>
+#include <reach/utils.h>
 #include <yaml-cpp/yaml.h>
 
 namespace reach_ros
@@ -37,7 +38,7 @@ JointPenaltyMoveIt::JointPenaltyMoveIt(moveit::core::RobotModelConstPtr model, c
 double JointPenaltyMoveIt::calculateScore(const std::map<std::string, double>& pose) const
 {
  // Pull the joints from the planning group out of the input pose map
- std::vector<double> pose_subset = utils::transcribeInputMap(pose, jmg_->getActiveJointModelNames());
+ std::vector<double> pose_subset = reach::extractSubset(pose, jmg_->getActiveJointModelNames());
  Eigen::Map<const Eigen::ArrayXd> min(joints_min_.data(), joints_min_.size());
  Eigen::Map<const Eigen::ArrayXd> max(joints_max_.data(), joints_max_.size());
  Eigen::Map<const Eigen::ArrayXd> joints(pose_subset.data(), pose_subset.size());

src/evaluation/manipulability_moveit.cpp

@@ -20,6 +20,7 @@
 #include <moveit/robot_model/joint_model_group.h>
 #include <numeric>
 #include <reach/plugin_utils.h>
+#include <reach/utils.h>
 #include <yaml-cpp/yaml.h>
 
 static std::vector<Eigen::Index> getJacobianRowSubset(const YAML::Node& config, const std::string& key = "jacobian_row_"
@@ -118,7 +119,7 @@ double ManipulabilityMoveIt::calculateScore(const std::map<std::string, double>&
  moveit::core::RobotState state(model_);
 
  // Take the subset of joints in the joint model group out of the input pose
- std::vector<double> pose_subset = utils::transcribeInputMap(pose, jmg_->getActiveJointModelNames());
+ std::vector<double> pose_subset = reach::extractSubset(pose, jmg_->getActiveJointModelNames());
  state.setJointGroupPositions(jmg_, pose_subset);
  state.update();
 

src/ik/moveit_ik_solver.cpp

@@ -19,6 +19,7 @@
 #include <moveit/common_planning_interface_objects/common_objects.h>
 #include <moveit/planning_scene/planning_scene.h>
 #include <reach/plugin_utils.h>
+#include <reach/utils.h>
 #include <yaml-cpp/yaml.h>
 
 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<std::vector<double>> MoveItIKSolver::solveIK(const Eigen::Isometry3d
 
  const std::vector<std::string>& joint_names = jmg_->getActiveJointModelNames();
 
- std::vector<double> seed_subset = utils::transcribeInputMap(seed, joint_names);
+ std::vector<double> seed_subset = reach::extractSubset(seed, joint_names);
  state.setJointGroupPositions(jmg_, seed_subset);
  state.update();
 

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);

src/utils.cpp

@@ -19,6 +19,7 @@
 #include <geometric_shapes/shape_operations.h>
 #include <geometric_shapes/shapes.h>
 #include <reach/types.h>
+#include <reach/utils.h>
 
 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<geometry_msgs::msg:
 std::vector<double> transcribeInputMap(const std::map<std::string, double>& input,
  const std::vector<std::string>& 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<double> 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