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)
+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
+   * @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,8 @@
+servo_solver_parameters:
+  reference_frame: {
+    type: string,
+    description: "Frame that is tracked by servo",
+    validation: {
+      not_empty<>: []
+    }
+  }

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;

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

@@ -0,0 +1,174 @@
+/*********************************************************************
+ * 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, Adam Pettinger
+ */
+
+#include <control_msgs/msg/joint_jog.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <moveit/local_constraint_solver_plugins/servo_solver.hpp>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/robot_state/conversions.h>
+#include <moveit/utils/logger.hpp>
+
+namespace moveit::hybrid_planning
+{
+namespace
+{
+rclcpp::Logger getLogger()
+{
+  return moveit::getLogger("servo_solver");
+}
+}  // namespace
+
+bool ServoSolver::initialize(const rclcpp::Node::SharedPtr& node,
+                             const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                             const std::string& /*group_name*/)
+{
+  planning_scene_monitor_ = planning_scene_monitor;
+  node_ = node;
+
+  const std::shared_ptr<const servo_solver_parameters::ParamListener> solver_param_listener =
+      std::make_shared<const servo_solver_parameters::ParamListener>(node, "");
+  solver_parameters_ = solver_param_listener->get_params();
+
+  // Get Servo Parameters
+  const std::string param_namespace = "moveit_servo";
+  const std::shared_ptr<const servo::ParamListener> servo_param_listener =
+      std::make_shared<const servo::ParamListener>(node, param_namespace);
+  servo_parameters_ = servo_param_listener->get_params();
+
+  // Create Servo and start it
+  servo_ = std::make_unique<moveit_servo::Servo>(node_, servo_param_listener, planning_scene_monitor_);
+
+  // Use for debugging
+  // twist_cmd_pub_ = node_->create_publisher<geometry_msgs::msg::TwistStamped>("~/delta_twist_cmds", 10);
+  return true;
+}
+
+bool ServoSolver::reset()
+{
+  RCLCPP_INFO(getLogger(), "Reset Servo Solver");
+  joint_cmd_rolling_window_.clear();
+  return true;
+};
+
+moveit_msgs::action::LocalPlanner::Feedback
+ServoSolver::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)
+{
+  // Create Feedback
+  moveit_msgs::action::LocalPlanner::Feedback feedback_result;
+
+  // Transform next robot trajectory waypoint into JointJog message
+  moveit_msgs::msg::RobotTrajectory robot_command;
+  local_trajectory.getRobotTrajectoryMsg(robot_command);
+
+  if (robot_command.joint_trajectory.points.empty())
+  {
+    feedback_result.feedback = std::string("Reference trajectory does not contain any points");
+    return feedback_result;
+  }
+
+  // Get current state
+  const auto current_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
+
+  // Create goal state
+  moveit::core::RobotState target_state(local_trajectory.getRobotModel());
+  target_state.setVariablePositions(robot_command.joint_trajectory.joint_names,
+                                    robot_command.joint_trajectory.points[0].positions);
+  target_state.update();
+
+  // TF planning_frame -> current EE
+  Eigen::Isometry3d current_pose = current_state->getFrameTransform(solver_parameters_.reference_frame);
+  // TF planning -> target EE
+  Eigen::Isometry3d target_pose = target_state.getFrameTransform(solver_parameters_.reference_frame);
+
+  // current EE -> planning frame * planning frame -> target EE
+  Eigen::Isometry3d diff_pose = current_pose.inverse() * target_pose;
+  Eigen::AngleAxisd axis_angle(diff_pose.linear());
+
+  constexpr double fixed_trans_vel = 0.05;
+  constexpr double fixed_rot_vel = 5;
+  const double trans_gain = fixed_trans_vel / diff_pose.translation().norm();
+  const double rot_gain = fixed_rot_vel / diff_pose.rotation().norm();
+
+  // Calculate Cartesian command delta
+  // Transform current pose to command frame
+  // Transform goal pose to command frame
+  servo_->setCommandType(moveit_servo::CommandType::TWIST);
+  moveit_servo::TwistCommand target_twist{
+    solver_parameters_.reference_frame,
+    { diff_pose.translation().x() * trans_gain, diff_pose.translation().y() * trans_gain,
+      diff_pose.translation().z() * trans_gain, axis_angle.axis().x() * axis_angle.angle() * rot_gain,
+      axis_angle.axis().y() * axis_angle.angle() * rot_gain, axis_angle.axis().z() * axis_angle.angle() * rot_gain }
+  };
+
+  std::optional<trajectory_msgs::msg::JointTrajectory> trajectory_msg;
+  // Create servo commands until a trajectory message can be generated
+  while (!trajectory_msg)
+  {
+    // Calculate next servo command
+    moveit_servo::KinematicState joint_state = servo_->getNextJointState(current_state, target_twist);
+    const auto status = servo_->getStatus();
+    // Servo solver feedback is always the status of the first servo iteration
+    if (feedback_result.feedback.empty() && status != moveit_servo::StatusCode::NO_WARNING)
+    {
+      feedback_result.feedback = moveit_servo::SERVO_STATUS_CODE_MAP.at(status);
+    }
+    // If servo couldn't compute the next joint state, exit local solver without a solution
+    if (status == moveit_servo::StatusCode::INVALID)
+    {
+      return feedback_result;
+    }
+    moveit_servo::updateSlidingWindow(joint_state, joint_cmd_rolling_window_, servo_parameters_.max_expected_latency,
+                                      node_->now());
+    if (!joint_cmd_rolling_window_.empty())
+    {
+      current_state->setJointGroupPositions(current_state->getJointModelGroup(servo_parameters_.move_group_name),
+                                            joint_cmd_rolling_window_.back().positions);
+      current_state->setJointGroupVelocities(current_state->getJointModelGroup(servo_parameters_.move_group_name),
+                                             joint_cmd_rolling_window_.back().velocities);
+    }
+    trajectory_msg = moveit_servo::composeTrajectoryMessage(servo_parameters_, joint_cmd_rolling_window_);
+  }
+  local_solution = trajectory_msg.value();
+  return feedback_result;
+}
+}  // namespace moveit::hybrid_planning
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(moveit::hybrid_planning::ServoSolver, moveit::hybrid_planning::LocalConstraintSolverInterface);