From 188f17c19e887bb5f5ac33b16994d84376032775 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 25 Jan 2024 10:54:54 +0100 Subject: [PATCH 1/4] Add servo solver plugin --- moveit_ros/hybrid_planning/CMakeLists.txt | 11 +- .../forward_trajectory_plugin.xml | 7 - .../CMakeLists.txt | 10 +- .../servo_solver.hpp | 92 +++++++++ .../src/servo_solver.cpp | 177 ++++++++++++++++++ .../moveit_planning_pipeline_plugin.xml | 7 - moveit_ros/hybrid_planning/package.xml | 2 + moveit_ros/hybrid_planning/plugins.xml | 46 +++++ .../replan_invalidated_trajectory_plugin.xml | 7 - .../hybrid_planning/simple_sampler_plugin.xml | 7 - .../single_plan_execution_plugin.xml | 7 - 11 files changed, 330 insertions(+), 43 deletions(-) delete mode 100644 moveit_ros/hybrid_planning/forward_trajectory_plugin.xml create mode 100644 moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/servo_solver.hpp create mode 100644 moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp delete mode 100644 moveit_ros/hybrid_planning/moveit_planning_pipeline_plugin.xml create mode 100644 moveit_ros/hybrid_planning/plugins.xml delete mode 100644 moveit_ros/hybrid_planning/replan_invalidated_trajectory_plugin.xml delete mode 100644 moveit_ros/hybrid_planning/simple_sampler_plugin.xml delete mode 100644 moveit_ros/hybrid_planning/single_plan_execution_plugin.xml diff --git a/moveit_ros/hybrid_planning/CMakeLists.txt b/moveit_ros/hybrid_planning/CMakeLists.txt index c62a293034..5eb9a9e23d 100644 --- a/moveit_ros/hybrid_planning/CMakeLists.txt +++ b/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,6 +32,7 @@ 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 @@ -38,10 +41,12 @@ set(LIBRARIES ) 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 +95,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) diff --git a/moveit_ros/hybrid_planning/forward_trajectory_plugin.xml b/moveit_ros/hybrid_planning/forward_trajectory_plugin.xml deleted file mode 100644 index 3bc40fe17d..0000000000 --- a/moveit_ros/hybrid_planning/forward_trajectory_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Simple local solver plugin that forwards the next waypoint of the sampled local trajectory. - - - diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt index fa68289600..302329eb96 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt @@ -1,5 +1,9 @@ -add_library(forward_trajectory_plugin SHARED - src/forward_trajectory.cpp -) +# Forward Trajectory Solver +add_library(forward_trajectory_plugin SHARED src/forward_trajectory.cpp) set_target_properties(forward_trajectory_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") ament_target_dependencies(forward_trajectory_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Servo Local Solver +add_library(servo_local_solver_plugin SHARED src/servo_solver.cpp) +set_target_properties(servo_local_solver_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(servo_local_solver_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/servo_solver.hpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/servo_solver.hpp new file mode 100644 index 0000000000..ac9a31af79 --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/servo_solver.hpp @@ -0,0 +1,92 @@ +/********************************************************************* + * 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 to execute the local trajectory in combination with + replanning capabilities + */ + +#pragma once + +#include +#include +#include + +#include + +namespace moveit::hybrid_planning +{ +class ServoSolver : public moveit::hybrid_planning::LocalConstraintSolverInterface +{ +public: + bool initialize(const rclcpp::Node::SharedPtr& node, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::string& group_name) override; + bool reset() override; + + moveit_msgs::action::LocalPlanner::Feedback + solve(const robot_trajectory::RobotTrajectory& local_trajectory, + const std::shared_ptr local_goal, + trajectory_msgs::msg::JointTrajectory& local_solution) override; + +private: + rclcpp::Node::SharedPtr node_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + servo::Params servo_parameters_; + + // Servo cpp interface + std::unique_ptr servo_; + + // Uncomment for debugging + // rclcpp::Publisher::SharedPtr twist_cmd_pub_; + + rclcpp::Publisher::SharedPtr traj_cmd_pub_; + bool publish_ = true; + rclcpp::Subscription::SharedPtr collision_velocity_scale_sub_; + + // Subscribe to laser corrections + rclcpp::Subscription::SharedPtr laser_corrections_sub_; + double laser_correction_ = 0; + + // Flag to indicate if replanning is necessary + bool replan_; + + // Flag to indicate that replanning is requested + bool feedback_send_; + + // Command queue to build trajectory message and add current robot state + std::deque joint_cmd_rolling_window_; +}; +} // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp new file mode 100644 index 0000000000..397dc96a2b --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp @@ -0,0 +1,177 @@ +/********************************************************************* + * 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 +#include +#include +#include +#include +#include + +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; + + // Get Servo Parameters + // Get the servo parameters. + const std::string param_namespace = "moveit_servo"; + const std::shared_ptr servo_param_listener = + std::make_shared(node, param_namespace); + servo_parameters_ = servo_param_listener->get_params(); + + // Create Servo and start it + servo_ = std::make_unique(node_, servo_param_listener, planning_scene_monitor_); + + // Use for debugging + // twist_cmd_pub_ = node_->create_publisher("~/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 /*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("tcp_welding_gun_link"); + // TF planning -> target EE + Eigen::Isometry3d target_pose = target_state.getFrameTransform("tcp_welding_gun_link"); + + // 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{ + "tcp_welding_gun_link", + { 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 } + }; + + // Start DEBUG uncomment for debugging + // auto msg = std::make_unique(); + // msg->header.stamp = node_->now(); + // msg->twist.linear.x = target_twist.velocities[0]; + // msg->twist.linear.y = target_twist.velocities[1]; + // msg->twist.linear.z = target_twist.velocities[2]; + // msg->twist.angular.x =target_twist.velocities[3]; + // msg->twist.angular.y =target_twist.velocities[4]; + // msg->twist.angular.z =target_twist.velocities[5]; + // twist_cmd_pub_->publish(std::move(msg)); + // End Debug + + std::optional trajectory_msg; + while (!trajectory_msg) + { + // Calculate next servo command + moveit_servo::KinematicState joint_state = servo_->getNextJointState(current_state, target_twist); + const auto status = servo_->getStatus(); + if (status == moveit_servo::StatusCode::INVALID) + { + feedback_result.feedback = "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_EXPORT_CLASS(moveit::hybrid_planning::ServoSolver, moveit::hybrid_planning::LocalConstraintSolverInterface); diff --git a/moveit_ros/hybrid_planning/moveit_planning_pipeline_plugin.xml b/moveit_ros/hybrid_planning/moveit_planning_pipeline_plugin.xml deleted file mode 100644 index 1b325280bd..0000000000 --- a/moveit_ros/hybrid_planning/moveit_planning_pipeline_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Use default MoveIt planning pipeline as Global Planner - - - diff --git a/moveit_ros/hybrid_planning/package.xml b/moveit_ros/hybrid_planning/package.xml index b35f8449a3..c85944884d 100644 --- a/moveit_ros/hybrid_planning/package.xml +++ b/moveit_ros/hybrid_planning/package.xml @@ -16,10 +16,12 @@ moveit_common ament_index_cpp + control_msgs moveit_msgs moveit_core moveit_ros_planning moveit_ros_planning_interface + moveit_servo pluginlib rclcpp rclcpp_action diff --git a/moveit_ros/hybrid_planning/plugins.xml b/moveit_ros/hybrid_planning/plugins.xml new file mode 100644 index 0000000000..178574053e --- /dev/null +++ b/moveit_ros/hybrid_planning/plugins.xml @@ -0,0 +1,46 @@ + + + + + + Simple hybrid planning logic that runs the global planner once and execute the global solution with the local planner. + + + + + + + Simple hybrid planning logic that runs the global planner once and starts executing the global solution with the local planner. In case the local planner detects a collision the global planner is rerun to update the invalidated global trajectory. + + + + + + + + Use default MoveIt planning pipeline as Global Planner + + + + + + + + Simple trajectory operator plugin that updates the local planner's reference trajectory by simple replacing it and extracts the next trajectory point based on the current robot state and an index. + + + + + + + + Simple local solver plugin that forwards the next waypoint of the sampled local trajectory. + + + + + + Uses Servo as a local solver. + + + \ No newline at end of file diff --git a/moveit_ros/hybrid_planning/replan_invalidated_trajectory_plugin.xml b/moveit_ros/hybrid_planning/replan_invalidated_trajectory_plugin.xml deleted file mode 100644 index 0bceaac42d..0000000000 --- a/moveit_ros/hybrid_planning/replan_invalidated_trajectory_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Simple hybrid planning logic that runs the global planner once and starts executing the global solution with the local planner. In case the local planner detects a collision the global planner is rerun to update the invalidated global trajectory. - - - diff --git a/moveit_ros/hybrid_planning/simple_sampler_plugin.xml b/moveit_ros/hybrid_planning/simple_sampler_plugin.xml deleted file mode 100644 index 3678ab0fef..0000000000 --- a/moveit_ros/hybrid_planning/simple_sampler_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Simple trajectory operator plugin that updates the local planner's reference trajectory by simple replacing it and extracts the next trajectory point based on the current robot state and an index. - - - diff --git a/moveit_ros/hybrid_planning/single_plan_execution_plugin.xml b/moveit_ros/hybrid_planning/single_plan_execution_plugin.xml deleted file mode 100644 index 847ec33f58..0000000000 --- a/moveit_ros/hybrid_planning/single_plan_execution_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Simple hybrid planning logic that runs the global planner once and execute the global solution with the local planner. - - - From 9ffda7a33b21dddafee765bdf8ab6eaf21295b62 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 25 Jan 2024 12:10:13 +0100 Subject: [PATCH 2/4] Cleanup + add description --- moveit_ros/hybrid_planning/CMakeLists.txt | 2 + .../CMakeLists.txt | 4 ++ .../servo_solver.hpp | 50 +++++++++++-------- .../res/forward_trajectory_parameters.yaml | 6 +++ .../res/servo_solver_parameters.yaml | 8 +++ .../src/forward_trajectory.cpp | 12 ++--- .../src/servo_solver.cpp | 30 ++++++----- moveit_ros/hybrid_planning/plugins.xml | 2 +- 8 files changed, 68 insertions(+), 46 deletions(-) create mode 100644 moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/forward_trajectory_parameters.yaml create mode 100644 moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml diff --git a/moveit_ros/hybrid_planning/CMakeLists.txt b/moveit_ros/hybrid_planning/CMakeLists.txt index 5eb9a9e23d..4c060bb5db 100644 --- a/moveit_ros/hybrid_planning/CMakeLists.txt +++ b/moveit_ros/hybrid_planning/CMakeLists.txt @@ -38,6 +38,8 @@ set(LIBRARIES # Parameters local_planner_parameters hp_manager_parameters + forward_trajectory_parameters + servo_solver_parameters ) set(THIS_PACKAGE_INCLUDE_DEPENDS diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt index 302329eb96..deff14b0e2 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt @@ -1,9 +1,13 @@ # 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}) diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/servo_solver.hpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/servo_solver.hpp index ac9a31af79..4e7b757b44 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/servo_solver.hpp +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/servo_solver.hpp @@ -33,8 +33,7 @@ *********************************************************************/ /* Author: Sebastian Jahr - Description: Local solver plugin that uses moveit_servo to execute the local trajectory in combination with - replanning capabilities + Description: Local solver plugin that uses moveit_servo steer the robot towards a Cartesian goal point. */ #pragma once @@ -44,17 +43,45 @@ #include #include +#include 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 local_goal, @@ -63,29 +90,12 @@ class ServoSolver : public moveit::hybrid_planning::LocalConstraintSolverInterfa 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 servo_; - // Uncomment for debugging - // rclcpp::Publisher::SharedPtr twist_cmd_pub_; - - rclcpp::Publisher::SharedPtr traj_cmd_pub_; - bool publish_ = true; - rclcpp::Subscription::SharedPtr collision_velocity_scale_sub_; - - // Subscribe to laser corrections - rclcpp::Subscription::SharedPtr laser_corrections_sub_; - double laser_correction_ = 0; - - // Flag to indicate if replanning is necessary - bool replan_; - - // Flag to indicate that replanning is requested - bool feedback_send_; - // Command queue to build trajectory message and add current robot state std::deque joint_cmd_rolling_window_; }; diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/forward_trajectory_parameters.yaml b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/forward_trajectory_parameters.yaml new file mode 100644 index 0000000000..588c003b14 --- /dev/null +++ b/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, + } \ No newline at end of file diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml new file mode 100644 index 0000000000..26fa05ad14 --- /dev/null +++ b/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<>: [] + #} + } \ No newline at end of file diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp index 13086091bc..a61f61e7cf 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp @@ -36,6 +36,7 @@ #include #include #include +#include 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("stop_before_collision", stop_before_collision_); - } - else - { - stop_before_collision_ = node->declare_parameter("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; diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp index 397dc96a2b..8f96f9b37c 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp @@ -59,6 +59,10 @@ bool ServoSolver::initialize(const rclcpp::Node::SharedPtr& node, planning_scene_monitor_ = planning_scene_monitor; node_ = node; + const std::shared_ptr solver_param_listener = + std::make_shared(node, ""); + solver_parameters_ = solver_param_listener->get_params(); + // Get Servo Parameters // Get the servo parameters. const std::string param_namespace = "moveit_servo"; @@ -109,9 +113,9 @@ ServoSolver::solve(const robot_trajectory::RobotTrajectory& local_trajectory, target_state.update(); // TF planning_frame -> current EE - Eigen::Isometry3d current_pose = current_state->getFrameTransform("tcp_welding_gun_link"); + Eigen::Isometry3d current_pose = current_state->getFrameTransform(solver_parameters_.reference_frame); // TF planning -> target EE - Eigen::Isometry3d target_pose = target_state.getFrameTransform("tcp_welding_gun_link"); + 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; @@ -127,33 +131,27 @@ ServoSolver::solve(const robot_trajectory::RobotTrajectory& local_trajectory, // Transform goal pose to command frame servo_->setCommandType(moveit_servo::CommandType::TWIST); moveit_servo::TwistCommand target_twist{ - "tcp_welding_gun_link", + 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 } }; - // Start DEBUG uncomment for debugging - // auto msg = std::make_unique(); - // msg->header.stamp = node_->now(); - // msg->twist.linear.x = target_twist.velocities[0]; - // msg->twist.linear.y = target_twist.velocities[1]; - // msg->twist.linear.z = target_twist.velocities[2]; - // msg->twist.angular.x =target_twist.velocities[3]; - // msg->twist.angular.y =target_twist.velocities[4]; - // msg->twist.angular.z =target_twist.velocities[5]; - // twist_cmd_pub_->publish(std::move(msg)); - // End Debug - std::optional 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()) + { + 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) { - feedback_result.feedback = "Servo StatusCode 'INVALID'"; return feedback_result; } moveit_servo::updateSlidingWindow(joint_state, joint_cmd_rolling_window_, servo_parameters_.max_expected_latency, diff --git a/moveit_ros/hybrid_planning/plugins.xml b/moveit_ros/hybrid_planning/plugins.xml index 178574053e..d4ffffaba4 100644 --- a/moveit_ros/hybrid_planning/plugins.xml +++ b/moveit_ros/hybrid_planning/plugins.xml @@ -43,4 +43,4 @@ Uses Servo as a local solver. - \ No newline at end of file + From 881e55c3e9616860a2c01d41de6e8ecf5e367f13 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 25 Jan 2024 12:30:24 +0100 Subject: [PATCH 3/4] Small update --- .../res/forward_trajectory_parameters.yaml | 2 +- .../res/servo_solver_parameters.yaml | 8 ++++---- .../local_constraint_solver_plugins/src/servo_solver.cpp | 3 +-- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/forward_trajectory_parameters.yaml b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/forward_trajectory_parameters.yaml index 588c003b14..181b982f33 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/forward_trajectory_parameters.yaml +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/forward_trajectory_parameters.yaml @@ -3,4 +3,4 @@ forward_trajectory_parameters: type: bool, description: "If a collision is detected should the robot stop at the current pose or continue executing", default_value: false, - } \ No newline at end of file + } diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml index 26fa05ad14..67646014d7 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml @@ -2,7 +2,7 @@ servo_solver_parameters: reference_frame: { type: string, description: "Frame that is tracked by servo", - #validation: { - # not_empty<>: [] - #} - } \ No newline at end of file + validation: { + not_empty<>: [] + } + } diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp index 8f96f9b37c..a351ee20fd 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp @@ -64,7 +64,6 @@ bool ServoSolver::initialize(const rclcpp::Node::SharedPtr& node, solver_parameters_ = solver_param_listener->get_params(); // Get Servo Parameters - // Get the servo parameters. const std::string param_namespace = "moveit_servo"; const std::shared_ptr servo_param_listener = std::make_shared(node, param_namespace); @@ -145,7 +144,7 @@ ServoSolver::solve(const robot_trajectory::RobotTrajectory& local_trajectory, 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()) + if (feedback_result.feedback.empty() && status != moveit_servo::StatusCode::NO_WARNING) { feedback_result.feedback = moveit_servo::SERVO_STATUS_CODE_MAP.at(status); } From 96160dab8d68bd565af4ceb19cbfc54e3b4f51cc Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 5 Feb 2024 11:39:45 +0100 Subject: [PATCH 4/4] Address review --- .../CMakeLists.txt | 2 +- .../servo_solver.hpp | 2 +- .../res/servo_solver_parameters.yaml | 16 +++++++ .../src/servo_solver.cpp | 42 +++++++++++++++---- 4 files changed, 52 insertions(+), 10 deletions(-) diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt index deff14b0e2..e24813635c 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt @@ -9,5 +9,5 @@ ament_target_dependencies(forward_trajectory_plugin ${THIS_PACKAGE_INCLUDE_DEPEN 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) +target_link_libraries(servo_local_solver_plugin servo_solver_parameters local_planner_parameters) ament_target_dependencies(servo_local_solver_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/servo_solver.hpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/servo_solver.hpp index 4e7b757b44..76463c30a9 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/servo_solver.hpp +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/servo_solver.hpp @@ -57,7 +57,7 @@ class ServoSolver : public moveit::hybrid_planning::LocalConstraintSolverInterfa /** * @brief Initializes moveit servo * - * @param node + * @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 diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml index 67646014d7..6fc5a0f1c3 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml @@ -6,3 +6,19 @@ servo_solver_parameters: 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 + } + } diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp index a351ee20fd..64094cafcc 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp @@ -41,6 +41,7 @@ #include #include #include +#include namespace moveit::hybrid_planning { @@ -72,8 +73,20 @@ bool ServoSolver::initialize(const rclcpp::Node::SharedPtr& node, // Create Servo and start it servo_ = std::make_unique(node_, servo_param_listener, planning_scene_monitor_); - // Use for debugging - // twist_cmd_pub_ = node_->create_publisher("~/delta_twist_cmds", 10); + // Set servo publish_period + auto local_param_listener = local_planner_parameters::ParamListener(node_, ""); + const auto local_config = local_param_listener.get_params(); + + // Publish period is the time difference used for numerical integration. It is recommended that the period 3x smaller + // than the actual publish period. + const double servo_publish_rate = 1.0 / 3.0 * local_config.local_planning_frequency; + if (!node_->has_parameter("publish_period")) + { + RCLCPP_ERROR(getLogger(), "Node used by servo solver doesn't seem to have a parameter called 'publish_period' that " + "shouldn't happen!"); + return false; + } + node_->set_parameter(rclcpp::Parameter("publish_period", servo_publish_rate)); return true; } @@ -106,9 +119,9 @@ ServoSolver::solve(const robot_trajectory::RobotTrajectory& local_trajectory, const auto current_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); // Create goal state - moveit::core::RobotState target_state(local_trajectory.getRobotModel()); + moveit::core::RobotState target_state = *current_state; target_state.setVariablePositions(robot_command.joint_trajectory.joint_names, - robot_command.joint_trajectory.points[0].positions); + robot_command.joint_trajectory.points.at(0).positions); target_state.update(); // TF planning_frame -> current EE @@ -120,10 +133,8 @@ ServoSolver::solve(const robot_trajectory::RobotTrajectory& local_trajectory, 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(); + const double trans_gain = solver_parameters_.trans_gain_scaling / diff_pose.translation().norm(); + const double rot_gain = solver_parameters_.rot_gain_scaling / diff_pose.rotation().norm(); // Calculate Cartesian command delta // Transform current pose to command frame @@ -137,6 +148,21 @@ ServoSolver::solve(const robot_trajectory::RobotTrajectory& local_trajectory, }; std::optional trajectory_msg; + + // Clear all commands that are older than older than one publish period + const auto cutoff_timestamp = node_->now() - rclcpp::Duration::from_seconds(3.0 * servo_parameters_.publish_period); + const auto cutoff_iterator = std::find_if(joint_cmd_rolling_window_.rbegin(), joint_cmd_rolling_window_.rend(), + [&cutoff_timestamp](moveit_servo::KinematicState state) { + return state.time_stamp < cutoff_timestamp; + }) + .base(); + + if (cutoff_iterator != joint_cmd_rolling_window_.end()) + { + // Erase elements from the beginning to the found position + joint_cmd_rolling_window_.erase(joint_cmd_rolling_window_.begin(), cutoff_iterator); + } + // Create servo commands until a trajectory message can be generated while (!trajectory_msg) {