Merge pull request moveit#1900 from Abishalini/pr-sync-1245f15

Sync with MoveIt1

moveit_core/robot_state/CMakeLists.txt

@@ -75,5 +75,6 @@ if(BUILD_TESTING)
   target_link_libraries(test_cartesian_interpolator
     moveit_test_utils
     moveit_robot_state
+    moveit_kinematics_base
   )
 endif()

moveit_core/utils/CMakeLists.txt

@@ -19,7 +19,7 @@ target_include_directories(moveit_test_utils PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include/moveit_core>
 )
-target_link_libraries(moveit_test_utils moveit_robot_model)
+target_link_libraries(moveit_test_utils moveit_robot_model moveit_kinematics_base)
 ament_target_dependencies(moveit_test_utils
   ament_index_cpp
   Boost

moveit_core/utils/include/moveit/utils/eigen_test_utils.h

@@ -0,0 +1,77 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2022, Bielefeld University
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Bielefeld University nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Robert Haschke */
+
+#include <gtest/gtest.h>
+#include <sstream>
+#include <Eigen/Geometry>
+
+/** Provide operator<< for Eigen::Transform */
+template <typename _Scalar, int _Dim, int _Mode, int _Options>
+std::ostream& operator<<(std::ostream& s, const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& t)
+{
+  return s << "p=[" << t.translation().transpose() << "] q=[" << Eigen::Quaterniond(t.linear()).coeffs().transpose()
+           << "]";
+}
+
+/** Predicate to compare two Eigen entities */
+template <typename T1, typename T2>
+struct IsApprox
+{
+  double prec_;
+  IsApprox(double prec_) : prec_(prec_)
+  {
+  }
+
+  ::testing::AssertionResult operator()(const char* expr1, const char* expr2, T1 val1, T2 val2)
+  {
+    if (val1.isApprox(val2, prec_))
+      return ::testing::AssertionSuccess();
+
+    std::stringstream msg;
+    msg << "Expected equality of these values (up to precision " << prec_ << "):" << std::fixed
+        << std::setprecision(1 - std::log10(prec_))        // limit to precision
+        << "\n  " << expr1 << "\n    Which is: " << val1   // first
+        << "\n  " << expr2 << "\n    Which is: " << val2;  // second
+    return ::testing::AssertionFailure() << msg.str();
+  }
+};
+
+#define EXPECT_EIGEN_EQ(val1, val2)                                                                                    \
+  EXPECT_PRED_FORMAT2((IsApprox<decltype(val1), decltype(val2)>(                                                       \
+                          Eigen::NumTraits<typename std::decay<decltype(val1)>::type::Scalar>::dummy_precision())),    \
+                      val1, val2)
+#define EXPECT_EIGEN_NEAR(val1, val2, prec_)                                                                           \
+  EXPECT_PRED_FORMAT2((IsApprox<decltype(val1), decltype(val2)>(prec_)), val1, val2)

moveit_core/utils/include/moveit/utils/robot_model_test_utils.h

@@ -42,6 +42,8 @@
 #include <moveit/robot_model/robot_model.h>
 #include <geometry_msgs/msg/pose.hpp>
 
+#include <rclcpp/node.hpp>
+
 namespace moveit
 {
 namespace core
@@ -70,6 +72,16 @@ urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name);
  */
 srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name);
 
+/** \brief Load an IK solver plugin for the given joint model group
+ * \param[in] jmg joint model group to load the plugin for
+ * \param[in] base_link base link of chain
+ * \param[in] tip_link tip link of chain
+ * \param[in] plugin name of the plugin ("KDL", or full name)
+ * \param[in] timeout default solver timeout
+ */
+void loadIKPluginForGroup(rclcpp::Node::SharedPtr node, JointModelGroup* jmg, const std::string& base_link,
+                          const std::string& tip_link, std::string plugin = "KDL", double timeout = 0.1);
+
 /** \brief Easily build different robot models for testing.
  *  Essentially a programmer-friendly light wrapper around URDF and SRDF.
  *  Best shown by an example:

moveit_core/utils/src/robot_model_test_utils.cpp

@@ -43,6 +43,10 @@
 #include <rclcpp/logging.hpp>
 #include <urdf_parser/urdf_parser.h>
 
+#include <pluginlib/class_loader.hpp>
+
+#include <moveit/kinematics_base/kinematics_base.h>
+
 namespace moveit
 {
 namespace core
@@ -100,6 +104,32 @@ srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name)
   return srdf_model;
 }
 
+void loadIKPluginForGroup(rclcpp::Node::SharedPtr node, JointModelGroup* jmg, const std::string& base_link,
+                          const std::string& tip_link, std::string plugin, double timeout)
+{
+  using LoaderType = pluginlib::ClassLoader<kinematics::KinematicsBase>;
+  static std::weak_ptr<LoaderType> cached_loader;
+  std::shared_ptr<LoaderType> loader = cached_loader.lock();
+  if (!loader)
+  {
+    loader = std::make_shared<LoaderType>("moveit_core", "kinematics::KinematicsBase");
+    cached_loader = loader;
+  }
+
+  // translate short to long names
+  if (plugin == "KDL")
+    plugin = "kdl_kinematics_plugin/KDLKinematicsPlugin";
+
+  jmg->setSolverAllocators(
+      [=](const JointModelGroup* jmg) -> kinematics::KinematicsBasePtr {
+        kinematics::KinematicsBasePtr result = loader->createUniqueInstance(plugin);
+        result->initialize(node, jmg->getParentModel(), jmg->getName(), base_link, { tip_link }, 0.0);
+        result->setDefaultTimeout(timeout);
+        return result;
+      },
+      SolverAllocatorMapFn());
+}
+
 RobotModelBuilder::RobotModelBuilder(const std::string& name, const std::string& base_link_name)
   : urdf_model_(std::make_shared<urdf::ModelInterface>()), srdf_writer_(std::make_shared<srdf::SRDFWriter>())
 {

moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp

@@ -1645,8 +1645,8 @@ double MoveGroupInterface::computeCartesianPath(const std::vector<geometry_msgs:
                                                 bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes* error_code)
 {
   moveit_msgs::msg::Constraints path_constraints_tmp;
-  return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions,
-                              error_code);
+  return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, moveit_msgs::msg::Constraints(),
+                              avoid_collisions, error_code);
 }
 
 double MoveGroupInterface::computeCartesianPath(const std::vector<geometry_msgs::msg::Pose>& waypoints, double eef_step,
@@ -1661,9 +1661,11 @@ double MoveGroupInterface::computeCartesianPath(const std::vector<geometry_msgs:
   }
   else
   {
-    moveit_msgs::msg::MoveItErrorCodes error_code_tmp;
+    moveit_msgs::msg::MoveItErrorCodes err_tmp;
+    err_tmp.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
+    moveit_msgs::msg::MoveItErrorCodes& err = error_code ? *error_code : err_tmp;
     return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
-                                       avoid_collisions, error_code_tmp);
+                                       avoid_collisions, err);
   }
 }