Path based approach refactoring

moveit_reach_plugins/CMakeLists.txt

@@ -76,8 +76,8 @@ pluginlib_export_plugin_description_file(reach_core eval_plugin_description.xml)
 add_library(ik_solver_plugins
   src/ik/moveit_ik_solver.cpp
   src/ik/discretized_moveit_ik_solver.cpp
-  src/ik/cartesian_retrieval_ik_solver.cpp
-  src/ik/planner_based_ik_solver.cpp
+#  src/ik/cartesian_retrieval_ik_solver.cpp
+#  src/ik/planner_based_ik_solver.cpp
 )
 target_include_directories(ik_solver_plugins
   PUBLIC
@@ -90,7 +90,6 @@ target_link_libraries(ik_solver_plugins
 ament_target_dependencies(ik_solver_plugins
   moveit_ros_planning_interface
   geometric_shapes
-#  reach_core
   pluginlib
   rclcpp
   tf2_eigen
@@ -114,6 +113,27 @@ target_link_libraries(reach_display_plugins
 )
 pluginlib_export_plugin_description_file(reach_core display_plugin_description.xml)
 
+# MoveIt Reach Path Plugin
+add_library(path_plugins
+  src/path/cartesian_path_generation.cpp
+)
+target_include_directories(path_plugins
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+target_link_libraries(path_plugins
+  ${PROJECT_NAME}_utils
+)
+ament_target_dependencies(path_plugins
+  moveit_ros_planning_interface
+  geometric_shapes
+  pluginlib
+  rclcpp
+  tf2_eigen
+)
+pluginlib_export_plugin_description_file(reach_core path_plugin_description.xml)
+
 # IK Plugin Test
 #add_executable(ik_plugin_test
 #  test/plugin_test_node.cpp
@@ -136,6 +156,7 @@ install(
     evaluation_plugins
     ik_solver_plugins
     reach_display_plugins
+    path_plugins
   EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
@@ -151,6 +172,7 @@ ament_export_libraries(
   evaluation_plugins
   ik_solver_plugins
   reach_display_plugins
+  path_plugins
 )
 
 ament_export_include_directories(

moveit_reach_plugins/ik_plugin_description.xml

@@ -13,16 +13,16 @@
       This plugin discretizes the target pose around the Z-axis and outputs the solution with the highest score
     </description>
   </class>
-  <class name="moveit_reach_plugins/ik/CartesianRetrievalIKSolver" type="moveit_reach_plugins::ik::CartesianRetrievalIKSolver" base_class_type="reach::plugins::IKSolverBase">
-    <description>
-      An inverse kinematics solver plugin which utilizes the MoveIt framework for solving robot inverse kinematics with respect to a given planning environment
-      This plugin additionally does retrieval path computation from the specified pose.
-    </description>
-  </class>
-  <class name="moveit_reach_plugins/ik/PlannerBasedIKSolver" type="moveit_reach_plugins::ik::PlannerBasedIKSolver" base_class_type="reach::plugins::IKSolverBase">
-    <description>
-      An inverse kinematics solver plugin which utilizes the MoveIt framework for solving robot inverse kinematics with respect to a given planning environment
-      This plugin additionally does path planning to the specified pose.
-    </description>
-  </class>
+<!--  <class name="moveit_reach_plugins/ik/CartesianRetrievalIKSolver" type="moveit_reach_plugins::ik::CartesianRetrievalIKSolver" base_class_type="reach::plugins::IKSolverBase">-->
+<!--    <description>-->
+<!--      An inverse kinematics solver plugin which utilizes the MoveIt framework for solving robot inverse kinematics with respect to a given planning environment-->
+<!--      This plugin additionally does retrieval path computation from the specified pose.-->
+<!--    </description>-->
+<!--  </class>-->
+<!--  <class name="moveit_reach_plugins/ik/PlannerBasedIKSolver" type="moveit_reach_plugins::ik::PlannerBasedIKSolver" base_class_type="reach::plugins::IKSolverBase">-->
+<!--    <description>-->
+<!--      An inverse kinematics solver plugin which utilizes the MoveIt framework for solving robot inverse kinematics with respect to a given planning environment-->
+<!--      This plugin additionally does path planning to the specified pose.-->
+<!--    </description>-->
+<!--  </class>-->
 </library>

moveit_reach_plugins/include/moveit_reach_plugins/ik/discretized_moveit_ik_solver.h

@@ -34,10 +34,8 @@ class DiscretizedMoveItIKSolver : public MoveItIKSolver {
 
   virtual std::optional<double> solveIKFromSeed(
       const Eigen::Isometry3d& target,
-      const std::map<std::string, double>& seed, std::vector<double>& solution,
-      std::vector<double>& joint_space_trajectory,
-      std::vector<double>& cartesian_space_waypoints, double& fraction,
-      moveit_msgs::msg::RobotTrajectory& moveit_trajectory) override;
+      const std::map<std::string, double>& seed,
+      std::vector<double>& solution) override;
 
  protected:
   double dt_;

moveit_reach_plugins/include/moveit_reach_plugins/ik/moveit_ik_solver.h

@@ -23,17 +23,6 @@
 // PlanningScene
 #include <moveit_msgs/msg/planning_scene.hpp>
 
-// namespace moveit
-//{
-// namespace core
-//{
-// class RobotModel;
-// typedef std::shared_ptr<const RobotModel> RobotModelConstPtr;
-// class JointModelGroup;
-// class RobotState;
-//}
-//}
-
 namespace planning_scene {
 class PlanningScene;
 typedef std::shared_ptr<PlanningScene> PlanningScenePtr;
@@ -63,10 +52,8 @@ class MoveItIKSolver : public reach::plugins::IKSolverBase {
 
   virtual std::optional<double> solveIKFromSeed(
       const Eigen::Isometry3d& target,
-      const std::map<std::string, double>& seed, std::vector<double>& solution,
-      std::vector<double>& joint_space_trajectory,
-      std::vector<double>& cartesian_space_waypoints, double& fraction,
-      moveit_msgs::msg::RobotTrajectory& moveit_trajectory) override;
+      const std::map<std::string, double>& seed,
+      std::vector<double>& solution) override;
 
   virtual std::vector<std::string> getJointNames() const override;
 

moveit_reach_plugins/include/moveit_reach_plugins/path/cartesian_path_generation.h

@@ -0,0 +1,90 @@
+/*
+ * Copyright 2022 PickNik, Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+/* Authors: Lovro Ivanov, @livanov93
+   Desc:
+*/
+#ifndef MOVEIT_REACH_PLUGINS_PATH_CARTESIAN_PATH_GENERATION_H
+#define MOVEIT_REACH_PLUGINS_PATH_CARTESIAN_PATH_GENERATION_H
+
+#include <pluginlib/class_loader.hpp>
+#include <reach_core/plugins/path_base.h>
+
+// PlanningScene
+#include <moveit_msgs/msg/planning_scene.hpp>
+
+// cartesian interpolator include
+#include "tf2_eigen/tf2_eigen.h"
+
+#include <moveit/robot_state/cartesian_interpolator.h>
+#include <moveit/robot_trajectory/robot_trajectory.h>
+
+namespace planning_scene {
+class PlanningScene;
+typedef std::shared_ptr<PlanningScene> PlanningScenePtr;
+}  // namespace planning_scene
+
+namespace moveit_reach_plugins {
+
+namespace path {
+
+class CartesianPathGeneration : public reach::plugins::PathBase {
+ public:
+  CartesianPathGeneration();
+
+  ~CartesianPathGeneration() {}
+
+  virtual bool initialize(
+      const std::string& name, rclcpp::Node::SharedPtr node,
+      const std::shared_ptr<const moveit::core::RobotModel> model) override;
+
+  virtual std::optional<double> solvePath(
+      const std::map<std::string, double>& start_state,
+      std::map<std::string, double>& end_state, double& fraction,
+      moveit_msgs::msg::RobotTrajectory& moveit_trajectory) override;
+
+  virtual std::vector<std::string> getJointNames() const override;
+
+  bool isIKSolutionValid(moveit::core::RobotState* state,
+                         const moveit::core::JointModelGroup* jmg,
+                         const double* ik_solution) const;
+
+ protected:
+  moveit::core::RobotModelConstPtr model_;
+  planning_scene::PlanningScenePtr scene_;
+  const moveit::core::JointModelGroup* jmg_;
+  std::string name_;
+
+  // distance to retrieve from ik solution in [m]
+  double retrieval_path_length_;
+  double jump_threshold_;
+  double max_eef_step_;
+  std::string tool_frame_;
+  double max_velocity_scaling_factor_;
+  double max_acceleration_scaling_factor_;
+  std::string collision_mesh_package_;
+  std::string collision_mesh_frame_;
+  // collision checking
+  double distance_threshold_;
+
+  // transformations
+  Eigen::Isometry3d global_transformation_;
+  Eigen::Isometry3d tool_transformation_;
+};
+
+}  // namespace path
+}  // namespace moveit_reach_plugins
+
+#endif  // MOVEIT_REACH_PLUGINS_PATH_CARTESIAN_PATH_GENERATION_H

moveit_reach_plugins/path_plugin_description.xml

@@ -0,0 +1,9 @@
+<!-- IK Plugins -->
+<library path="path_plugins">
+  <!-- MoveIt IK Solver -->
+  <class name="moveit_reach_plugins/path/CartesianPathGeneration" type="moveit_reach_plugins::path::CartesianPathGeneration" base_class_type="reach::plugins::PathBase">
+    <description>
+      Cartesian Interpolator plugin
+    </description>
+  </class>
+</library>

moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp

@@ -64,9 +64,7 @@ bool DiscretizedMoveItIKSolver::initialize(
 
 std::optional<double> DiscretizedMoveItIKSolver::solveIKFromSeed(
     const Eigen::Isometry3d& target, const std::map<std::string, double>& seed,
-    std::vector<double>& solution, std::vector<double>& joint_space_trajectory,
-    std::vector<double>& cartesian_space_waypoints, double& fraction,
-    moveit_msgs::msg::RobotTrajectory& moveit_trajectory) {
+    std::vector<double>& solution) {
   // Calculate the number of discretizations necessary to achieve discretization
   // angle
   const static int n_discretizations = int((2.0 * M_PI) / dt_);
@@ -79,9 +77,8 @@ std::optional<double> DiscretizedMoveItIKSolver::solveIKFromSeed(
     Eigen::Isometry3d discretized_target(
         target * Eigen::AngleAxisd(double(i) * dt_, Eigen::Vector3d::UnitZ()));
     std::vector<double> tmp_solution;
-    std::optional<double> score = MoveItIKSolver::solveIKFromSeed(
-        discretized_target, seed, tmp_solution, joint_space_trajectory,
-        cartesian_space_waypoints, fraction, moveit_trajectory);
+    std::optional<double> score =
+        MoveItIKSolver::solveIKFromSeed(discretized_target, seed, tmp_solution);
     if (score.has_value() && (score.value() > best_score)) {
       best_score = score.value();
       best_solution = std::move(tmp_solution);
@@ -94,7 +91,6 @@ std::optional<double> DiscretizedMoveItIKSolver::solveIKFromSeed(
     solution = std::move(best_solution);
     return std::optional<double>(best_score);
   } else {
-    fraction = 0.0;
     return {};
   }
 }

moveit_reach_plugins/src/ik/moveit_ik_solver.cpp

@@ -76,8 +76,6 @@ bool MoveItIKSolver::initialize(
     return false;
   }
 
-  //    model_ = moveit::planning_interface::getSharedRobotModelLoader(node,
-  //    "robot_description")->getModel();
   model_ = model;
 
   if (!model_) {
@@ -123,9 +121,7 @@ bool MoveItIKSolver::initialize(
 
 std::optional<double> MoveItIKSolver::solveIKFromSeed(
     const Eigen::Isometry3d& target, const std::map<std::string, double>& seed,
-    std::vector<double>& solution, std::vector<double>& joint_space_trajectory,
-    std::vector<double>& cartesian_space_waypoints, double& fraction,
-    moveit_msgs::msg::RobotTrajectory& moveit_trajectory) {
+    std::vector<double>& solution) {
   moveit::core::RobotState state(model_);
 
   const std::vector<std::string>& joint_names =
@@ -149,7 +145,6 @@ std::optional<double> MoveItIKSolver::solveIKFromSeed(
                       std::bind(&MoveItIKSolver::isIKSolutionValid, this,
                                 std::placeholders::_1, std::placeholders::_2,
                                 std::placeholders::_3))) {
-    fraction = 1.0;
     solution.clear();
     state.copyJointGroupPositions(jmg_, solution);
 
@@ -161,7 +156,6 @@ std::optional<double> MoveItIKSolver::solveIKFromSeed(
 
     return eval_->calculateScore(solution_map);
   } else {
-    fraction = 0.0;
     return {};
   }
 }
@@ -178,15 +172,6 @@ bool MoveItIKSolver::isIKSolutionValid(moveit::core::RobotState* state,
       (scene_->distanceToCollision(
            *state, scene_->getAllowedCollisionMatrix()) < distance_threshold_);
 
-  // visualization part for debugging purposes only
-  /*
-    if (!colliding && !too_close){
-        scene_->setCurrentState(*state);
-        moveit_msgs::msg::PlanningScene scene_msg;
-        scene_->getPlanningSceneMsg(scene_msg);
-        scene_pub_->publish(scene_msg);
-    }
-    */
   return (!colliding && !too_close);
 }