Add servo solver

moveit_ros/hybrid_planning/CMakeLists.txt

@@ -7,11 +7,13 @@ moveit_package()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(control_msgs REQUIRED)
 find_package(generate_parameter_library REQUIRED)
 find_package(moveit_core REQUIRED)
 find_package(moveit_msgs REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
+find_package(moveit_servo REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
@@ -30,18 +32,23 @@ set(LIBRARIES
   forward_trajectory_plugin
   motion_planning_pipeline_plugin
   replan_invalidated_trajectory_plugin
+  servo_local_solver_plugin
   simple_sampler_plugin
   single_plan_execution_plugin
   # Parameters
   local_planner_parameters
   hp_manager_parameters
+  forward_trajectory_parameters
+  servo_solver_parameters
 )
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
+  control_msgs
   moveit_core
   moveit_msgs
   moveit_ros_planning
   moveit_ros_planning_interface
+  moveit_servo
   pluginlib
   rclcpp
   rclcpp_action
@@ -90,11 +97,7 @@ install(DIRECTORY ${THIS_PACKAGE_INCLUDE_DIRS} DESTINATION include/moveit_hybrid
 install(DIRECTORY test/launch DESTINATION share/moveit_hybrid_planning)
 install(DIRECTORY test/config DESTINATION share/moveit_hybrid_planning)
 
-pluginlib_export_plugin_description_file(moveit_hybrid_planning single_plan_execution_plugin.xml)
-pluginlib_export_plugin_description_file(moveit_hybrid_planning moveit_planning_pipeline_plugin.xml)
-pluginlib_export_plugin_description_file(moveit_hybrid_planning simple_sampler_plugin.xml)
-pluginlib_export_plugin_description_file(moveit_hybrid_planning replan_invalidated_trajectory_plugin.xml)
-pluginlib_export_plugin_description_file(moveit_hybrid_planning forward_trajectory_plugin.xml)
+pluginlib_export_plugin_description_file(moveit_hybrid_planning plugins.xml)
 
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 ament_export_targets(moveit_hybrid_planningTargets HAS_LIBRARY_TARGET)

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt

@@ -1,5 +1,13 @@
-add_library(forward_trajectory_plugin SHARED
-  src/forward_trajectory.cpp
-)
+# Forward Trajectory Solver
+generate_parameter_library(forward_trajectory_parameters res/forward_trajectory_parameters.yaml)
+add_library(forward_trajectory_plugin SHARED src/forward_trajectory.cpp)
 set_target_properties(forward_trajectory_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+target_link_libraries(forward_trajectory_plugin forward_trajectory_parameters)
 ament_target_dependencies(forward_trajectory_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# Servo Local Solver
+generate_parameter_library(servo_solver_parameters res/servo_solver_parameters.yaml)
+add_library(servo_local_solver_plugin SHARED src/servo_solver.cpp)
+set_target_properties(servo_local_solver_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+target_link_libraries(servo_local_solver_plugin servo_solver_parameters local_planner_parameters)
+ament_target_dependencies(servo_local_solver_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS})

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/servo_solver.hpp

@@ -0,0 +1,102 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021, PickNik Inc.
+ *  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 PickNik Inc. 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: Sebastian Jahr
+   Description: Local solver plugin that uses moveit_servo steer the robot towards a Cartesian goal point.
+ */
+
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <moveit/local_planner/local_constraint_solver_interface.h>
+
+#include <moveit_servo/servo.hpp>
+#include <servo_solver_parameters.hpp>
+
+namespace moveit::hybrid_planning
+{
+/**
+ * @brief Local solver plugin that utilizes moveit_servo as a local planner
+ *
+ */
+class ServoSolver : public moveit::hybrid_planning::LocalConstraintSolverInterface
+{
+public:
+  /**
+   * @brief Initializes moveit servo
+   *
+   * @param node Node provided by the local planning component to this plugin. Can be used to init and use ROS interfaces
+   * @param planning_scene_monitor Planning scene monitor to access the current state of the system
+   * @param group_name UNUSED
+   * @return True if initialization was successful
+   */
+  bool initialize(const rclcpp::Node::SharedPtr& node,
+                  const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                  const std::string& group_name) override;
+  /**
+   * @brief Reset rolling window
+   *
+   * @return Always returns true
+   */
+  bool reset() override;
+
+  /**
+   * @brief Solves local planning problem with servo. The first waypoint of the local trajectory is used as goal point
+   * for servo. After computing FK for that waypoint, the difference between the current and goal reference frame is used
+   * to calculate a twist command for servo. The status of servo is returned as feedback to the hybrid planning manager.
+   *
+   * @param local_trajectory Reference trajectory, only the first waypoint is used
+   * @param local_goal UNUSED
+   * @param local_solution Joint trajectory containing a sequence of servo commands
+   * @return moveit_msgs::action::LocalPlanner::Feedback containing the servo status code
+   */
+  moveit_msgs::action::LocalPlanner::Feedback
+  solve(const robot_trajectory::RobotTrajectory& local_trajectory,
+        const std::shared_ptr<const moveit_msgs::action::LocalPlanner::Goal> local_goal,
+        trajectory_msgs::msg::JointTrajectory& local_solution) override;
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
+  servo_solver_parameters::Params solver_parameters_;
+  servo::Params servo_parameters_;
+
+  // Servo cpp interface
+  std::unique_ptr<moveit_servo::Servo> servo_;
+
+  // Command queue to build trajectory message and add current robot state
+  std::deque<moveit_servo::KinematicState> joint_cmd_rolling_window_;
+};
+}  // namespace moveit::hybrid_planning

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/forward_trajectory_parameters.yaml

@@ -0,0 +1,6 @@
+forward_trajectory_parameters:
+  stop_before_collision: {
+    type: bool,
+    description: "If a collision is detected should the robot stop at the current pose or continue executing",
+    default_value: false,
+  }

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml

@@ -0,0 +1,24 @@
+servo_solver_parameters:
+  reference_frame: {
+    type: string,
+    description: "Frame that is tracked by servo",
+    validation: {
+      not_empty<>: []
+    }
+  }
+  trans_gain_scaling: {
+    type: double,
+    description: "Scaling for translative velocities computed from pose diff.",
+    default_value: 0.05,
+    validation: {
+      gt<>: 0.0
+    }
+  }
+  rot_gain_scaling: {
+    type: double,
+    description: "Scaling for rotational velocities computed from pose diff",
+    default_value: 5.0,
+    validation: {
+      gt<>: 0.0
+    }
+  }

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp

@@ -36,6 +36,7 @@
 #include <moveit/local_planner/feedback_types.h>
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit/robot_state/conversions.h>
+#include <forward_trajectory_parameters.hpp>
 
 namespace
 {
@@ -50,15 +51,8 @@ bool ForwardTrajectory::initialize(const rclcpp::Node::SharedPtr& node,
                                    const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
                                    const std::string& /* unused */)
 {
-  // Load parameter & initialize member variables
-  if (node->has_parameter("stop_before_collision"))
-  {
-    node->get_parameter<bool>("stop_before_collision", stop_before_collision_);
-  }
-  else
-  {
-    stop_before_collision_ = node->declare_parameter<bool>("stop_before_collision", false);
-  }
+  auto param_listener = forward_trajectory_parameters::ParamListener(node, "");
+  stop_before_collision_ = param_listener.get_params().stop_before_collision;
   node_ = node;
   path_invalidation_event_send_ = false;
   num_iterations_stuck_ = 0;