diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index dc9fccbf96..e00b8a7eb3 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -7,7 +7,6 @@ moveit_package() set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs - control_toolbox geometry_msgs moveit_core moveit_msgs @@ -42,60 +41,59 @@ include_directories( # This library provides a way of loading parameters for servo generate_parameter_library( moveit_servo_lib_parameters - src/servo_parameters.yaml + config/servo_parameters.yaml ) # This library provides a C++ interface for sending realtime twist or joint commands to a robot -add_library(moveit_servo_lib SHARED - src/collision_check.cpp +add_library(moveit_servo_lib_cpp SHARED + src/collision_monitor.cpp src/servo.cpp - src/servo_calcs.cpp - src/utilities.cpp -) -set_target_properties(moveit_servo_lib PROPERTIES VERSION "${moveit_servo_VERSION}") -ament_target_dependencies(moveit_servo_lib ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(moveit_servo_lib moveit_servo_lib_parameters) + src/utils/common.cpp + src/utils/command.cpp -add_library(pose_tracking SHARED src/pose_tracking.cpp) -ament_target_dependencies(pose_tracking ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(pose_tracking moveit_servo_lib) +) +set_target_properties(moveit_servo_lib_cpp PROPERTIES VERSION "${moveit_servo_VERSION}") +target_link_libraries(moveit_servo_lib_cpp moveit_servo_lib_parameters) +ament_target_dependencies(moveit_servo_lib_cpp ${THIS_PACKAGE_INCLUDE_DEPENDS}) -##################### -## Component Nodes ## -##################### +add_library(moveit_servo_lib_ros SHARED src/servo_node.cpp) +set_target_properties(moveit_servo_lib_ros PROPERTIES VERSION "${moveit_servo_VERSION}") +target_link_libraries(moveit_servo_lib_ros moveit_servo_lib_cpp) +ament_target_dependencies(moveit_servo_lib_ros ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Add and export library to run as a ROS node component, and receive commands via topics -add_library(servo_node SHARED src/servo_node.cpp) -ament_target_dependencies(servo_node ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(servo_node moveit_servo_lib) -rclcpp_components_register_nodes(servo_node "moveit_servo::ServoNode") +###################### +## Components ## +###################### -# Add executable for using a controller -add_library(servo_controller_input SHARED src/teleop_demo/joystick_servo_example.cpp) -ament_target_dependencies(servo_controller_input PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -rclcpp_components_register_nodes(servo_controller_input "moveit_servo::JoyToServoPub") +rclcpp_components_register_node( + moveit_servo_lib_ros + PLUGIN "moveit_servo::ServoNode" + EXECUTABLE servo_node +) ###################### ## Executable Nodes ## ###################### -# An executable node for the servo server -add_executable(servo_node_main src/servo_node_main.cpp) -target_link_libraries(servo_node_main servo_node) -ament_target_dependencies(servo_node_main ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Executable node for the joint jog demo +add_executable(demo_joint_jog demos/cpp_interface/demo_joint_jog.cpp ) +target_link_libraries(demo_joint_jog moveit_servo_lib_cpp) +ament_target_dependencies(demo_joint_jog ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# An example of pose tracking -add_executable(servo_pose_tracking_demo src/cpp_interface_demo/pose_tracking_demo.cpp) -target_link_libraries(servo_pose_tracking_demo pose_tracking) -ament_target_dependencies(servo_pose_tracking_demo ${THIS_PACKAGE_INCLUDE_DEPENDS}) +# Executable node for the twist demo +add_executable(demo_twist demos/cpp_interface/demo_twist.cpp ) +target_link_libraries(demo_twist moveit_servo_lib_cpp) +ament_target_dependencies(demo_twist ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Add executable to publish fake servo commands for testing/demo purposes -add_executable(fake_command_publisher test/publish_fake_jog_commands.cpp) -ament_target_dependencies(fake_command_publisher - rclcpp - geometry_msgs - std_srvs -) +# Executable node for the pose demo +add_executable(demo_pose demos/cpp_interface/demo_pose.cpp ) +target_link_libraries(demo_pose moveit_servo_lib_cpp) +ament_target_dependencies(demo_pose ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Keyboard control example for servo +add_executable(servo_keyboard_input demos/servo_keyboard_input.cpp) +target_include_directories(servo_keyboard_input PUBLIC include) +ament_target_dependencies(servo_keyboard_input ${THIS_PACKAGE_INCLUDE_DEPENDS}) ############# ## Install ## @@ -104,11 +102,9 @@ ament_target_dependencies(fake_command_publisher # Install Libraries install( TARGETS - moveit_servo_lib + moveit_servo_lib_cpp + moveit_servo_lib_ros moveit_servo_lib_parameters - pose_tracking - servo_node - servo_controller_input EXPORT moveit_servoTargets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib @@ -119,9 +115,11 @@ install( # Install Binaries install( TARGETS - servo_node_main - servo_pose_tracking_demo - fake_command_publisher + demo_joint_jog + demo_twist + demo_pose + servo_node + servo_keyboard_input ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/moveit_servo @@ -135,56 +133,25 @@ install(DIRECTORY config DESTINATION share/moveit_servo) ament_export_targets(moveit_servoTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -############# -## TESTING ## -############# if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(ament_cmake_gtest REQUIRED) - find_package(ros_testing REQUIRED) - find_package(Boost REQUIRED COMPONENTS filesystem) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_cmake_flake8_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - - # Run all lint tests in package.xml except those listed above - ament_lint_auto_find_test_dependencies() - - # Servo integration launch test - ament_add_gtest_executable(test_servo_integration - test/test_servo_interface.cpp - test/servo_launch_test_common.hpp - ) - target_link_libraries(test_servo_integration moveit_servo_lib_parameters) - ament_target_dependencies(test_servo_integration ${THIS_PACKAGE_INCLUDE_DEPENDS}) - add_ros_test(test/launch/test_servo_integration.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - - # Servo collision checking integration test - ament_add_gtest_executable(test_servo_collision - test/test_servo_collision.cpp - test/servo_launch_test_common.hpp - ) - target_link_libraries(test_servo_collision moveit_servo_lib_parameters) - ament_target_dependencies(test_servo_collision ${THIS_PACKAGE_INCLUDE_DEPENDS}) - add_ros_test(test/launch/test_servo_collision.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - - # pose_tracking - ament_add_gtest_executable(test_servo_pose_tracking - test/pose_tracking_test.cpp - ) - ament_target_dependencies(test_servo_pose_tracking ${THIS_PACKAGE_INCLUDE_DEPENDS}) - target_link_libraries(test_servo_pose_tracking pose_tracking) - add_ros_test(test/launch/test_servo_pose_tracking.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - - # Unit tests - ament_add_gtest(servo_calcs_unit_tests - test/servo_calcs_unit_tests.cpp - ) - target_link_libraries(servo_calcs_unit_tests moveit_servo_lib) + + find_package(ament_cmake_gtest REQUIRED) + find_package(ros_testing REQUIRED) + + ament_add_gtest_executable(moveit_servo_utils_test tests/test_utils.cpp) + target_link_libraries(moveit_servo_utils_test moveit_servo_lib_cpp) + ament_target_dependencies(moveit_servo_utils_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) + add_ros_test(tests/launch/servo_utils.test.py TIMEOUT 30 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + + ament_add_gtest_executable(moveit_servo_cpp_integration_test tests/test_integration.cpp tests/servo_cpp_fixture.hpp) + target_link_libraries(moveit_servo_cpp_integration_test moveit_servo_lib_cpp) + ament_target_dependencies(moveit_servo_cpp_integration_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) + add_ros_test(tests/launch/servo_cpp_integration.test.py TIMEOUT 30 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + + ament_add_gtest_executable(moveit_servo_ros_integration_test tests/test_ros_integration.cpp tests/servo_ros_fixture.hpp) + ament_target_dependencies(moveit_servo_ros_integration_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) + add_ros_test(tests/launch/servo_ros_integration.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() diff --git a/moveit_ros/moveit_servo/config/demo_rviz_config.rviz b/moveit_ros/moveit_servo/config/demo_rviz_config.rviz index 8bbbaccbc4..ad183c48c5 100644 --- a/moveit_ros/moveit_servo/config/demo_rviz_config.rviz +++ b/moveit_ros/moveit_servo/config/demo_rviz_config.rviz @@ -46,7 +46,7 @@ Visualization Manager: Enabled: true Move Group Namespace: "" Name: PlanningScene - Planning Scene Topic: /servo_node/publish_planning_scene + Planning Scene Topic: /moveit_servo_demo/publish_planning_scene Robot Description: robot_description Scene Geometry: Scene Alpha: 0.8999999761581421 diff --git a/moveit_ros/moveit_servo/config/demo_rviz_pose_tracking.rviz b/moveit_ros/moveit_servo/config/demo_rviz_config_ros.rviz similarity index 99% rename from moveit_ros/moveit_servo/config/demo_rviz_pose_tracking.rviz rename to moveit_ros/moveit_servo/config/demo_rviz_config_ros.rviz index d93be94cde..8bbbaccbc4 100644 --- a/moveit_ros/moveit_servo/config/demo_rviz_pose_tracking.rviz +++ b/moveit_ros/moveit_servo/config/demo_rviz_config_ros.rviz @@ -46,7 +46,7 @@ Visualization Manager: Enabled: true Move Group Namespace: "" Name: PlanningScene - Planning Scene Topic: /monitored_planning_scene + Planning Scene Topic: /servo_node/publish_planning_scene Robot Description: robot_description Scene Geometry: Scene Alpha: 0.8999999761581421 diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml index e65036135a..0189a505b0 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml @@ -2,8 +2,13 @@ # Modify all parameters related to servoing here ############################################### -## Properties of incoming commands -command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) +# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] + +## Properties of outgoing commands +publish_period: 0.034 # 1/Nominal publish rate [seconds] + +command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. @@ -11,13 +16,6 @@ scale: # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. joint: 0.5 -# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) -# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] - -## Properties of outgoing commands -publish_period: 0.034 # 1/Nominal publish rate [seconds] -low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) - # What type of topic does your robot driver expect? # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory command_out_type: trajectory_msgs/JointTrajectory @@ -28,6 +26,7 @@ publish_joint_velocities: true publish_joint_accelerations: false ## Plugins for smoothing outgoing commands +use_smoothing: true smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, @@ -41,11 +40,7 @@ move_group_name: panda_arm # Often 'manipulator' or 'arm' planning_frame: panda_link0 # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames -ee_frame_name: panda_link8 # The name of the end effector link, used to return the EE pose -robot_link_command_frame: panda_link0 # commands must be given in the frame of a robot link. Usually either the base or end effector - -## Stopping behaviour -incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command +ee_frame: panda_link8 # The name of the end effector link, used to return the EE pose ## Configure handling of singularities and joint limits lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) diff --git a/moveit_ros/moveit_servo/config/pose_tracking_settings.yaml b/moveit_ros/moveit_servo/config/pose_tracking_settings.yaml deleted file mode 100644 index e4b803774d..0000000000 --- a/moveit_ros/moveit_servo/config/pose_tracking_settings.yaml +++ /dev/null @@ -1,21 +0,0 @@ -################################# -# PID parameters for pose seeking -################################# - -# Maximum value of error integral for all PID controllers -windup_limit: 0.05 - -# PID gains -x_proportional_gain: 1.5 -y_proportional_gain: 1.5 -z_proportional_gain: 1.5 -x_integral_gain: 0.0 -y_integral_gain: 0.0 -z_integral_gain: 0.0 -x_derivative_gain: 0.0 -y_derivative_gain: 0.0 -z_derivative_gain: 0.0 - -angular_proportional_gain: 0.5 -angular_integral_gain: 0.0 -angular_derivative_gain: 0.0 diff --git a/moveit_ros/moveit_servo/src/servo_parameters.yaml b/moveit_ros/moveit_servo/config/servo_parameters.yaml similarity index 69% rename from moveit_ros/moveit_servo/src/servo_parameters.yaml rename to moveit_ros/moveit_servo/config/servo_parameters.yaml index 774a085991..aaa12cca47 100644 --- a/moveit_ros/moveit_servo/src/servo_parameters.yaml +++ b/moveit_ros/moveit_servo/config/servo_parameters.yaml @@ -14,16 +14,9 @@ servo: must be passed to the node during launch time." } - ee_frame_name: { + ee_frame: { type: string, - description: "The name of end-effector frame of your robot \ - This parameter does not have a default value and \ - must be passed to the node during launch time." - } - - robot_link_command_frame: { - type: string, - description: "The frame of incoming command, change this to your own value when using a different robot \ + description: "The name of end effector frame of your robot \ This parameter does not have a default value and \ must be passed to the node during launch time." } @@ -59,6 +52,13 @@ servo: } ################################ INPUTS ############################# + + pose_command_in_topic: { + type: string, + default_value: "~/pose_target_cmds", + description: "The topic on which servo will receive the pose commands" + } + cartesian_command_in_topic: { type: string, default_value: "~/delta_twist_cmds", @@ -74,7 +74,9 @@ servo: command_in_type: { type: string, default_value: "unitless", - description: "The unit of the incoming command.", + description: "The unit of the incoming command. \ + unitless commands are in the range [-1:1] as if from joystick \ + speed_units are in m/s and rad/s", validation: { one_of<>: [["unitless", "speed_units"]] } @@ -87,13 +89,23 @@ servo: } ################################ GENERAL CONFIG ############################# - enable_parameter_update: { - type: bool, - default_value: false, - description: "If true, continuous check for parameter update is enabled \ - Currently only the following parameters are updated: - 1. override_velocity_scaling_factor - 2. robot_link_command_frame" + + thread_priority: { + type: int, + default_value: 40, + description: "This value is used when configuring the servo loop thread to use SCHED_FIFO scheduling \ + We use a slightly lower priority than the ros2_control default in order to reduce jitter \ + Reference: https://man7.org/linux/man-pages/man2/sched_setparam.2.html", + validation: { + gt_eq<>: 0 + } + } + + incoming_command_timeout: { + type: double, + default_value: 0.1, + description: "Commands will be discarded if it is older than the timeout.\ + Important because ROS may drop some messages." } scale: @@ -146,6 +158,13 @@ servo: description: "If true servo will publish joint accelerations in the output command" } + + use_smoothing: { + type: bool, + default_value: true, + description: "Enables the use of smoothing plugins for joint trajectory smoothing" + } + smoothing_filter_plugin_name: { type: string, default_value: "online_signal_smoothing::ButterworthFilterPlugin", @@ -162,15 +181,6 @@ servo: this should be set to false" } - incoming_command_timeout: { - type: double, - default_value: 0.1, - description: "Stop servoing if X seconds elapse without a new command. \ - If 0, republish commands forever even if the robot is stationary. \ - Otherwise, specify num. to publish. Important because ROS may drop \ - some messages and we need the robot to halt reliably." - } - halt_all_joints_in_joint_mode: { type: bool, default_value: true, @@ -212,6 +222,15 @@ servo: } } + singularity_step_scale: { + type: double, + default_value: 0.01, + description: "The vector towards singularity is scaled by this factor during singularity check", + validation: { + gt<>: 0.0 + } + } + joint_limit_margin: { type: double, default_value: 0.1, @@ -221,12 +240,6 @@ servo: } } - low_latency_mode: { - type: bool, - default_value: false, - description: "If true , low latency mode is enabled." - } - check_collisions: { type: bool, default_value: true, @@ -261,82 +274,24 @@ servo: } } -############### POSE TRACKING PARAMETERS ######################### - - windup_limit: { - type: double, - default_value: 0.05, - description: "Maximum value of error integral for all PID controllers" - } - - x_proportional_gain: { - type: double, - default_value: 1.5, - description: "Proportional gain value for the controller in x direction" - } - - y_proportional_gain: { - type: double, - default_value: 1.5, - description: "Proportional gain value for the controller in y direction" - } - - z_proportional_gain: { - type: double, - default_value: 1.5, - description: "Proportional gain value for the controller in z direction" - } - - x_integral_gain: { - type: double, - default_value: 0.0, - description: "Integral gain value for the controller in x direction" - } - - y_integral_gain: { - type: double, - default_value: 0.0, - description: "Integral gain value for the controller in y direction" - } - z_integral_gain: { - type: double, - default_value: 0.0, - description: "Integral gain value for the controller in z direction" - } +########################### POSE TRACKING ####################################### - x_derivative_gain: { - type: double, - default_value: 0.0, - description: "Derivative gain value for the controller in x direction" - } - - y_derivative_gain: { - type: double, - default_value: 0.0, - description: "Derivative gain value for the controller in y direction" - } - - z_derivative_gain: { - type: double, - default_value: 0.0, - description: "Derivative gain value for the controller in z direction" - } - - angular_proportional_gain: { - type: double, - default_value: 0.5, - description: "Proportional gain value for angular control" - } - - angular_integral_gain: { - type: double, - default_value: 0.0, - description: "Integral gain value for angular control" - } + pose_tracking: + linear_tolerance: { + type: double, + default_value: 0.001, + description: "The allowable linear error when tracking a pose.", + validation: { + gt<>: 0.0 + } + } - angular_derivative_gain: { - type: double, - default_value: 0.0, - description: "Derivative gain value for angular control" - } + angular_tolerance: { + type: double, + default_value: 0.01, + description: "The allowable angular error when tracking a pose.", + validation: { + gt<>: 0.0 + } + } diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml b/moveit_ros/moveit_servo/config/test_config_panda.yaml similarity index 73% rename from moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml rename to moveit_ros/moveit_servo/config/test_config_panda.yaml index d0ee8d6b40..97a12cd3e9 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml +++ b/moveit_ros/moveit_servo/config/test_config_panda.yaml @@ -2,20 +2,20 @@ # Modify all parameters related to servoing here ############################################### -## Properties of incoming commands -command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s -scale: - # Scale parameters are only used if command_in_type=="unitless" - linear: 0.6 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.3 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. - # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. - joint: 0.01 - # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] ## Properties of outgoing commands -publish_period: 0.034 # 1/Nominal publish rate [seconds] +publish_period: 0.02 # 1/Nominal publish rate [seconds] + +incoming_command_timeout: 0.5 # seconds +command_in_type: "unitless" # "unitless" in the range [-1:1], as if from joystick. "speed_units" cmds are in m/s and rad/s +scale: + # Scale parameters are only used if command_in_type=="unitless" + linear: 1.0 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + rotational: 1.0 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. + joint: 1.0 # What type of topic does your robot driver expect? # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory @@ -27,18 +27,21 @@ publish_joint_velocities: true publish_joint_accelerations: false ## Plugins for smoothing outgoing commands +use_smoothing: false smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" +# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, +# which other nodes can use as a source for information about the planning environment. +# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), +# then is_primary_planning_scene_monitor needs to be set to false. +is_primary_planning_scene_monitor: true + ## MoveIt properties move_group_name: panda_arm # Often 'manipulator' or 'arm' planning_frame: panda_link0 # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames -ee_frame_name: panda_hand # The name of the end effector link, used to return the EE pose -robot_link_command_frame: panda_hand # commands must be given in the frame of a robot link. Usually either the base or end effector - -## Stopping behaviour -incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command +ee_frame: panda_link8 # The name of the end effector link, used to return the EE pose ## Configure handling of singularities and joint limits lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp new file mode 100644 index 0000000000..237f3f715a --- /dev/null +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp @@ -0,0 +1,115 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2023, 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 the copyright holder 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 HOLDER 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. + *******************************************************************************/ + +/* Title : demo_joint_jog.cpp + * Project : moveit_servo + * Created : 05/27/2023 + * Author : V Mohammed Ibrahim + * + * Description : Example of controlling a robot through joint jog commands via the C++ API. + */ + +#include +#include +#include +#include + +using namespace moveit_servo; + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.joint_jog_demo"); +} + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + // The servo object expects to get a ROS node. + const rclcpp::Node::SharedPtr demo_node = std::make_shared("moveit_servo_demo"); + + // Get the servo parameters. + const std::string param_namespace = "moveit_servo"; + const std::shared_ptr servo_param_listener = + std::make_shared(demo_node, param_namespace); + const servo::Params servo_params = servo_param_listener->get_params(); + + // The publisher to send trajectory message to the robot controller. + rclcpp::Publisher::SharedPtr trajectory_outgoing_cmd_pub = + demo_node->create_publisher(servo_params.command_out_topic, + rclcpp::SystemDefaultsQoS()); + + // Create the servo object + const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = + createPlanningSceneMonitor(demo_node, servo_params); + Servo servo = Servo(demo_node, servo_param_listener, planning_scene_monitor); + + // Wait for some time, so that the planning scene is loaded in rviz. + // This is just for convenience, should not be used for sync in real application. + std::this_thread::sleep_for(std::chrono::seconds(3)); + + // Set the command type for servo. + servo.setCommandType(CommandType::JOINT_JOG); + // JointJog command that moves only the 7th joint at +1.0 rad/s + JointJogCommand joint_jog{ { "panda_joint7" }, { 1.0 } }; + + // Frequency at which commands will be sent to the robot controller. + rclcpp::WallRate rate(1.0 / servo_params.publish_period); + + std::chrono::seconds timeout_duration(3); + std::chrono::seconds time_elapsed(0); + auto start_time = std::chrono::steady_clock::now(); + + RCLCPP_INFO_STREAM(LOGGER, servo.getStatusMessage()); + while (rclcpp::ok()) + { + const KinematicState joint_state = servo.getNextJointState(joint_jog); + const StatusCode status = servo.getStatus(); + + auto current_time = std::chrono::steady_clock::now(); + time_elapsed = std::chrono::duration_cast(current_time - start_time); + if (time_elapsed > timeout_duration) + { + RCLCPP_INFO_STREAM(LOGGER, "Timed out"); + break; + } + else if (status != StatusCode::INVALID) + { + trajectory_outgoing_cmd_pub->publish(composeTrajectoryMessage(servo_params, joint_state)); + } + rate.sleep(); + } + + RCLCPP_INFO(LOGGER, "Exiting demo."); + rclcpp::shutdown(); +} diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp new file mode 100644 index 0000000000..bfb5e92ad7 --- /dev/null +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp @@ -0,0 +1,159 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2023, 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 the copyright holder 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 HOLDER 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. + *******************************************************************************/ + +/* Title : demo_pose.cpp + * Project : moveit_servo + * Created : 06/07/2023 + * Author : V Mohammed Ibrahim + * Description : Example of controlling a robot through pose commands via the C++ API. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace moveit_servo; + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.pose_demo"); +} + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + // The servo object expects to get a ROS node. + const rclcpp::Node::SharedPtr demo_node = std::make_shared("moveit_servo_demo"); + + // Get the servo parameters. + const std::string param_namespace = "moveit_servo"; + const std::shared_ptr servo_param_listener = + std::make_shared(demo_node, param_namespace); + const servo::Params servo_params = servo_param_listener->get_params(); + + // The publisher to send trajectory message to the robot controller. + rclcpp::Publisher::SharedPtr trajectory_outgoing_cmd_pub = + demo_node->create_publisher(servo_params.command_out_topic, + rclcpp::SystemDefaultsQoS()); + + // Create the servo object + const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = + createPlanningSceneMonitor(demo_node, servo_params); + Servo servo = Servo(demo_node, servo_param_listener, planning_scene_monitor); + + // Wait for some time, so that the planning scene is loaded in rviz. + // This is just for convenience, should not be used for sync in real application. + std::this_thread::sleep_for(std::chrono::seconds(3)); + + // For syncing pose tracking thread and main thread. + std::mutex pose_guard; + std::atomic stop_tracking = false; + + // Set the command type for servo. + servo.setCommandType(CommandType::POSE); + + // The dynamically updated target pose. + PoseCommand target_pose; + target_pose.frame_id = servo_params.planning_frame; + // Initializing the target pose as end effector pose, this can be any pose. + target_pose.pose = servo.getEndEffectorPose(); + + // The pose tracking lambda that will be run in a separate thread. + auto pose_tracker = [&]() { + KinematicState joint_state; + rclcpp::WallRate tracking_rate(1 / servo_params.publish_period); + while (!stop_tracking && rclcpp::ok()) + { + { + std::lock_guard pguard(pose_guard); + joint_state = servo.getNextJointState(target_pose); + } + StatusCode status = servo.getStatus(); + if (status != StatusCode::INVALID) + trajectory_outgoing_cmd_pub->publish(composeTrajectoryMessage(servo_params, joint_state)); + + tracking_rate.sleep(); + } + }; + + // Pose tracking thread will exit upon reaching this pose. + Eigen::Isometry3d terminal_pose = target_pose.pose; + terminal_pose.rotate(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); + terminal_pose.translate(Eigen::Vector3d(0.0, 0.0, -0.1)); + + std::thread tracker_thread(pose_tracker); + tracker_thread.detach(); + + // The target pose (frame being tracked) moves by this step size each iteration. + Eigen::Vector3d linear_step_size{ 0.0, 0.0, -0.002 }; + Eigen::AngleAxisd angular_step_size(0.01, Eigen::Vector3d::UnitZ()); + + // Frequency at which commands will be sent to the robot controller. + rclcpp::WallRate command_rate(50); + RCLCPP_INFO_STREAM(LOGGER, servo.getStatusMessage()); + + while (!stop_tracking && rclcpp::ok()) + { + { + std::lock_guard pguard(pose_guard); + target_pose.pose = servo.getEndEffectorPose(); + const bool satisfies_linear_tolerance = target_pose.pose.translation().isApprox( + terminal_pose.translation(), servo_params.pose_tracking.linear_tolerance); + const bool satisfies_angular_tolerance = + target_pose.pose.rotation().isApprox(terminal_pose.rotation(), servo_params.pose_tracking.angular_tolerance); + stop_tracking = satisfies_linear_tolerance && satisfies_angular_tolerance; + // Dynamically update the target pose. + if (!satisfies_linear_tolerance) + target_pose.pose.translate(linear_step_size); + if (!satisfies_angular_tolerance) + target_pose.pose.rotate(angular_step_size); + } + + command_rate.sleep(); + } + + RCLCPP_INFO_STREAM(LOGGER, "REACHED : " << stop_tracking); + stop_tracking = true; + + if (tracker_thread.joinable()) + tracker_thread.join(); + + RCLCPP_INFO(LOGGER, "Exiting demo."); + rclcpp::shutdown(); +} diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp new file mode 100644 index 0000000000..3bc53ca9fd --- /dev/null +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp @@ -0,0 +1,119 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2023, 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 the copyright holder 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 HOLDER 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. + *******************************************************************************/ + +/* Title : demo_twist.cpp + * Project : moveit_servo + * Created : 06/01/2023 + * Author : V Mohammed Ibrahim + * + * Description : Example of controlling a robot through twist commands via the C++ API. + */ + +#include +#include +#include +#include +#include +#include + +using namespace moveit_servo; + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.twist_demo"); +} + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + // The servo object expects to get a ROS node. + const rclcpp::Node::SharedPtr demo_node = std::make_shared("moveit_servo_demo"); + + // Get the servo parameters. + const std::string param_namespace = "moveit_servo"; + const std::shared_ptr servo_param_listener = + std::make_shared(demo_node, param_namespace); + const servo::Params servo_params = servo_param_listener->get_params(); + + // The publisher to send trajectory message to the robot controller. + rclcpp::Publisher::SharedPtr trajectory_outgoing_cmd_pub = + demo_node->create_publisher(servo_params.command_out_topic, + rclcpp::SystemDefaultsQoS()); + + // Create the servo object + const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = + createPlanningSceneMonitor(demo_node, servo_params); + Servo servo = Servo(demo_node, servo_param_listener, planning_scene_monitor); + + // Wait for some time, so that the planning scene is loaded in rviz. + // This is just for convenience, should not be used for sync in real application. + std::this_thread::sleep_for(std::chrono::seconds(3)); + + // Set the command type for servo. + servo.setCommandType(CommandType::TWIST); + + // Move end effector in the +z direction at 10 cm/s + // while turning around z axis in the +ve direction at 0.5 rad/s + TwistCommand target_twist{ servo_params.planning_frame, { 0.0, 0.0, 0.1, 0.0, 0.0, 0.5 } }; + + // Frequency at which commands will be sent to the robot controller. + rclcpp::WallRate rate(1.0 / servo_params.publish_period); + + std::chrono::seconds timeout_duration(5); + std::chrono::seconds time_elapsed(0); + auto start_time = std::chrono::steady_clock::now(); + + RCLCPP_INFO_STREAM(LOGGER, servo.getStatusMessage()); + while (rclcpp::ok()) + { + const KinematicState joint_state = servo.getNextJointState(target_twist); + const StatusCode status = servo.getStatus(); + + auto current_time = std::chrono::steady_clock::now(); + time_elapsed = std::chrono::duration_cast(current_time - start_time); + if (time_elapsed > timeout_duration) + { + RCLCPP_INFO_STREAM(LOGGER, "Timed out"); + break; + } + else if (status != StatusCode::INVALID) + { + trajectory_outgoing_cmd_pub->publish(composeTrajectoryMessage(servo_params, joint_state)); + } + rate.sleep(); + } + + RCLCPP_INFO(LOGGER, "Exiting demo."); + rclcpp::shutdown(); +} diff --git a/moveit_ros/moveit_servo/demos/servo_keyboard_input.cpp b/moveit_ros/moveit_servo/demos/servo_keyboard_input.cpp new file mode 100644 index 0000000000..dd865c6408 --- /dev/null +++ b/moveit_ros/moveit_servo/demos/servo_keyboard_input.cpp @@ -0,0 +1,350 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, PickNik LLC + * 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 LLC 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. + *********************************************************************/ + +/* Title : servo_keyboard_input.cpp + * Project : moveit_servo + * Created : 05/31/2021 + * Author : Adam Pettinger, V Mohammed Ibrahim + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Define used keys +namespace +{ +constexpr int8_t KEYCODE_RIGHT = 0x43; +constexpr int8_t KEYCODE_LEFT = 0x44; +constexpr int8_t KEYCODE_UP = 0x41; +constexpr int8_t KEYCODE_DOWN = 0x42; +constexpr int8_t KEYCODE_PERIOD = 0x2E; +constexpr int8_t KEYCODE_SEMICOLON = 0x3B; +constexpr int8_t KEYCODE_1 = 0x31; +constexpr int8_t KEYCODE_2 = 0x32; +constexpr int8_t KEYCODE_3 = 0x33; +constexpr int8_t KEYCODE_4 = 0x34; +constexpr int8_t KEYCODE_5 = 0x35; +constexpr int8_t KEYCODE_6 = 0x36; +constexpr int8_t KEYCODE_7 = 0x37; +constexpr int8_t KEYCODE_Q = 0x71; +constexpr int8_t KEYCODE_R = 0x72; +constexpr int8_t KEYCODE_J = 0x6A; +constexpr int8_t KEYCODE_T = 0x74; +} // namespace + +// Some constants used in the Servo Teleop demo +namespace +{ +const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds"; +const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds"; +const size_t ROS_QUEUE_SIZE = 10; +const std::string PLANNING_FRAME_ID = "panda_link0"; +} // namespace + +// A class for reading the key inputs from the terminal +class KeyboardReader +{ +public: + KeyboardReader() : file_descriptor_(0) + { + // get the console in raw mode + tcgetattr(file_descriptor_, &cooked_); + struct termios raw; + memcpy(&raw, &cooked_, sizeof(struct termios)); + raw.c_lflag &= ~(ICANON | ECHO); + // Setting a new line, then end of file + raw.c_cc[VEOL] = 1; + raw.c_cc[VEOF] = 2; + tcsetattr(file_descriptor_, TCSANOW, &raw); + } + void readOne(char* c) + { + int rc = read(file_descriptor_, c, 1); + if (rc < 0) + { + throw std::runtime_error("read failed"); + } + } + void shutdown() + { + tcsetattr(file_descriptor_, TCSANOW, &cooked_); + } + +private: + int file_descriptor_; + struct termios cooked_; +}; + +// Converts key-presses to Twist or Jog commands for Servo, in lieu of a controller +class KeyboardServo +{ +public: + KeyboardServo(); + int keyLoop(); + +private: + void spin(); + + rclcpp::Node::SharedPtr nh_; + + rclcpp::Publisher::SharedPtr twist_pub_; + rclcpp::Publisher::SharedPtr joint_pub_; + rclcpp::Client::SharedPtr switch_input_; + + std::shared_ptr request_; + double joint_vel_cmd_; +}; + +KeyboardServo::KeyboardServo() : joint_vel_cmd_(1.0) +{ + nh_ = rclcpp::Node::make_shared("servo_keyboard_input"); + + twist_pub_ = nh_->create_publisher(TWIST_TOPIC, ROS_QUEUE_SIZE); + joint_pub_ = nh_->create_publisher(JOINT_TOPIC, ROS_QUEUE_SIZE); + + // Client for switching input types + switch_input_ = nh_->create_client("servo_node/switch_command_type"); +} + +KeyboardReader input; + +void quit(int sig) +{ + (void)sig; + input.shutdown(); + rclcpp::shutdown(); + exit(0); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + KeyboardServo keyboard_servo; + + signal(SIGINT, quit); + + int rc = keyboard_servo.keyLoop(); + input.shutdown(); + rclcpp::shutdown(); + + return rc; +} + +void KeyboardServo::spin() +{ + while (rclcpp::ok()) + { + rclcpp::spin_some(nh_); + } +} + +int KeyboardServo::keyLoop() +{ + char c; + bool publish_twist = false; + bool publish_joint = false; + + std::thread{ [this]() { return spin(); } }.detach(); + + puts("Reading from keyboard"); + puts("---------------------------"); + puts("All commands are in the planning frame"); + puts("Use arrow keys and the '.' and ';' keys to Cartesian jog"); + puts("Use 1|2|3|4|5|6|7 keys to joint jog. 'r' to reverse the direction of jogging."); + puts("Use 'j' to select joint jog. "); + puts("Use 't' to select twist "); + puts("'Q' to quit."); + + for (;;) + { + // get the next event from the keyboard + try + { + input.readOne(&c); + } + catch (const std::runtime_error&) + { + perror("read():"); + return -1; + } + + RCLCPP_DEBUG(nh_->get_logger(), "value: 0x%02X\n", c); + + // // Create the messages we might publish + auto twist_msg = std::make_unique(); + auto joint_msg = std::make_unique(); + + joint_msg->joint_names.resize(7); + joint_msg->joint_names = { "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", + "panda_joint5", "panda_joint6", "panda_joint7" }; + + joint_msg->velocities.resize(7); + std::fill(joint_msg->velocities.begin(), joint_msg->velocities.end(), 0.0); + // Use read key-press + switch (c) + { + case KEYCODE_LEFT: + RCLCPP_DEBUG(nh_->get_logger(), "LEFT"); + twist_msg->twist.linear.y = -0.5; + publish_twist = true; + break; + case KEYCODE_RIGHT: + RCLCPP_DEBUG(nh_->get_logger(), "RIGHT"); + twist_msg->twist.linear.y = 0.5; + publish_twist = true; + break; + case KEYCODE_UP: + RCLCPP_DEBUG(nh_->get_logger(), "UP"); + twist_msg->twist.linear.x = 0.5; + publish_twist = true; + break; + case KEYCODE_DOWN: + RCLCPP_DEBUG(nh_->get_logger(), "DOWN"); + twist_msg->twist.linear.x = -0.5; + publish_twist = true; + break; + case KEYCODE_PERIOD: + RCLCPP_DEBUG(nh_->get_logger(), "PERIOD"); + twist_msg->twist.linear.z = -0.5; + publish_twist = true; + break; + case KEYCODE_SEMICOLON: + RCLCPP_DEBUG(nh_->get_logger(), "SEMICOLON"); + twist_msg->twist.linear.z = 0.5; + publish_twist = true; + break; + case KEYCODE_1: + RCLCPP_DEBUG(nh_->get_logger(), "1"); + joint_msg->velocities[0] = joint_vel_cmd_; + publish_joint = true; + break; + case KEYCODE_2: + RCLCPP_DEBUG(nh_->get_logger(), "2"); + joint_msg->velocities[1] = joint_vel_cmd_; + publish_joint = true; + break; + case KEYCODE_3: + RCLCPP_DEBUG(nh_->get_logger(), "3"); + joint_msg->velocities[2] = joint_vel_cmd_; + publish_joint = true; + break; + case KEYCODE_4: + RCLCPP_DEBUG(nh_->get_logger(), "4"); + joint_msg->velocities[3] = joint_vel_cmd_; + publish_joint = true; + break; + case KEYCODE_5: + RCLCPP_DEBUG(nh_->get_logger(), "5"); + joint_msg->velocities[4] = joint_vel_cmd_; + publish_joint = true; + break; + case KEYCODE_6: + RCLCPP_DEBUG(nh_->get_logger(), "6"); + joint_msg->velocities[5] = joint_vel_cmd_; + publish_joint = true; + break; + case KEYCODE_7: + RCLCPP_DEBUG(nh_->get_logger(), "7"); + joint_msg->velocities[6] = joint_vel_cmd_; + publish_joint = true; + break; + case KEYCODE_R: + RCLCPP_DEBUG(nh_->get_logger(), "r"); + joint_vel_cmd_ *= -1; + break; + case KEYCODE_J: + RCLCPP_DEBUG(nh_->get_logger(), "j"); + request_ = std::make_shared(); + request_->command_type = moveit_msgs::srv::ServoCommandType::Request::JOINT_JOG; + if (switch_input_->wait_for_service(std::chrono::seconds(1))) + { + auto result = switch_input_->async_send_request(request_); + if (result.get()->success) + { + RCLCPP_INFO_STREAM(nh_->get_logger(), "Switched to input type: JointJog"); + } + else + { + RCLCPP_WARN_STREAM(nh_->get_logger(), "Could not switch input to: JointJog"); + } + } + break; + case KEYCODE_T: + RCLCPP_DEBUG(nh_->get_logger(), "t"); + request_ = std::make_shared(); + request_->command_type = moveit_msgs::srv::ServoCommandType::Request::TWIST; + if (switch_input_->wait_for_service(std::chrono::seconds(1))) + { + auto result = switch_input_->async_send_request(request_); + if (result.get()->success) + { + RCLCPP_INFO_STREAM(nh_->get_logger(), "Switched to input type: Twist"); + } + else + { + RCLCPP_WARN_STREAM(nh_->get_logger(), "Could not switch input to: Twist"); + } + } + break; + case KEYCODE_Q: + RCLCPP_DEBUG(nh_->get_logger(), "quit"); + return 0; + } + + // If a key requiring a publish was pressed, publish the message now + if (publish_twist) + { + twist_msg->header.stamp = nh_->now(); + twist_msg->header.frame_id = PLANNING_FRAME_ID; + twist_pub_->publish(std::move(twist_msg)); + publish_twist = false; + } + else if (publish_joint) + { + joint_msg->header.stamp = nh_->now(); + joint_msg->header.frame_id = PLANNING_FRAME_ID; + joint_pub_->publish(std::move(joint_msg)); + publish_joint = false; + } + } + + return 0; +} diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h deleted file mode 100644 index 22f3db24f7..0000000000 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h +++ /dev/null @@ -1,120 +0,0 @@ -/******************************************************************************* - * Title : collision_check.h - * Project : moveit_servo - * Created : 1/11/2019 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 copyright holder 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 HOLDER 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. - *******************************************************************************/ - -#pragma once - -#include - -#include - -#include -#include -#include -#include -// Auto-generated -#include - -namespace moveit_servo -{ -class CollisionCheck -{ -public: - /** \brief Constructor - * \param parameters: common settings of moveit_servo - * \param planning_scene_monitor: PSM should have scene monitor and state monitor - * already started when passed into this class - */ - CollisionCheck(const rclcpp::Node::SharedPtr& node, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& servo_param_listener); - - ~CollisionCheck() - { - stop(); - } - - /** \brief Start the Timer that regulates collision check rate */ - void start(); - - /** \brief Stop the Timer that regulates collision check rate */ - void stop(); - -private: - /** \brief Run one iteration of collision checking */ - void run(); - - /** \brief Get a read-only copy of the planning scene */ - planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const; - - // Pointer to the ROS node - const std::shared_ptr node_; - - // Servo parameters - const std::shared_ptr servo_param_listener_; - servo::Params servo_params_; - - // Pointer to the collision environment - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - - // Robot state and collision matrix from planning scene - std::shared_ptr current_state_; - - // Scale robot velocity according to collision proximity and user-defined thresholds. - // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold. - // Proximity decreasing --> decelerate - double velocity_scale_ = 1.0; - double self_collision_distance_ = 0.0; - double scene_collision_distance_ = 0.0; - bool collision_detected_ = false; - - const double self_velocity_scale_coefficient_; - const double scene_velocity_scale_coefficient_; - - // collision request - collision_detection::CollisionRequest collision_request_; - collision_detection::CollisionResult collision_result_; - - // ROS - rclcpp::TimerBase::SharedPtr timer_; - double period_; // The loop period, in seconds - rclcpp::Publisher::SharedPtr collision_velocity_scale_pub_; - - mutable std::mutex joint_state_mutex_; - sensor_msgs::msg::JointState latest_joint_state_; -}; -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp similarity index 51% rename from moveit_ros/moveit_servo/include/moveit_servo/servo.h rename to moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp index a18909dea2..34e020376c 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp @@ -1,9 +1,4 @@ /******************************************************************************* - * Title : servo.h - * Project : moveit_servo - * Created : 3/9/2017 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - * * BSD 3-Clause License * * Copyright (c) 2019, Los Alamos National Security, LLC @@ -35,71 +30,60 @@ * 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. *******************************************************************************/ +/* + * Title : collision_monitor.hpp + * Project : moveit_servo + * Created : 06/08/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + * + * Description: Monitors the planning scene for collision and publishes the velocity scaling. + */ #pragma once -// System -#include - -// Moveit2 -#include -#include #include +#include +#include namespace moveit_servo { -/** - * Class Servo - Jacobian based robot control with collision avoidance. - */ -class Servo + +class CollisionMonitor { public: - Servo(const rclcpp::Node::SharedPtr& node, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& servo_param_listener); + CollisionMonitor(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const servo::Params& servo_params, std::atomic& collision_velocity_scale); - /** \brief Start servo node */ void start(); - /** \brief Stop servo node */ void stop(); +private: /** - * Get the MoveIt planning link transform. - * The transform from the MoveIt planning frame to robot_link_command_frame - * - * @param transform the transform that will be calculated - * @return true if a valid transform was available + * \brief The collision checking function, this will run in a separate thread. */ - bool getCommandFrameTransform(Eigen::Isometry3d& transform); - bool getCommandFrameTransform(geometry_msgs::msg::TransformStamped& transform); + void checkCollisions(); - /** - * Get the End Effector link transform. - * The transform from the MoveIt planning frame to EE link - * - * @param transform the transform that will be calculated - * @return true if a valid transform was available - */ - bool getEEFrameTransform(Eigen::Isometry3d& transform); - bool getEEFrameTransform(geometry_msgs::msg::TransformStamped& transform); + // Variables - // Give test access to private/protected methods - friend class ServoFixture; + const servo::Params& servo_params_; + moveit::core::RobotStatePtr robot_state_; + const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; -private: - // Servo parameters - const std::shared_ptr servo_param_listener_; - servo::Params servo_params_; + // The collision monitor thread. + std::thread monitor_thread_; + // The flag used for stopping the collision monitor thread. + std::atomic stop_requested_; - // Pointer to the collision environment - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + // The scaling factor when approaching a collision. + std::atomic& collision_velocity_scale_; - ServoCalcs servo_calcs_; - CollisionCheck collision_checker_; + // The data structures used to get information about robot self collisions. + collision_detection::CollisionRequest self_collision_request_; + collision_detection::CollisionResult self_collision_result_; + // The data structures used to get information about robot collision with other objects in the collision scene. + collision_detection::CollisionRequest scene_collision_request_; + collision_detection::CollisionResult scene_collision_result_; }; -// ServoPtr using alias -using ServoPtr = std::shared_ptr; - } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h b/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h deleted file mode 100644 index fa0436a3a6..0000000000 --- a/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h +++ /dev/null @@ -1,57 +0,0 @@ -/******************************************************************************* - * Title : make_shared_from_pool.h - * Project : moveit_servo - * Created : 1/11/2019 - * Author : Tyler Weaver - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 copyright holder 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 HOLDER 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. - *******************************************************************************/ - -#pragma once - -#include -#include - -namespace moveit -{ -namespace util -{ -// Useful template for creating messages from a message pool -template -std::shared_ptr make_shared_from_pool() -{ - using allocator_t = boost::fast_pool_allocator>; - return std::allocate_shared(allocator_t()); -} - -} // namespace util -} // namespace moveit diff --git a/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h b/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h deleted file mode 100644 index c99d239a61..0000000000 --- a/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h +++ /dev/null @@ -1,195 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, 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: Andy Zelenak - Desc: Servoing. Track a pose setpoint in real time. -*/ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Conventions: -// Calculations are done in the planning_frame_ unless otherwise noted. - -namespace moveit_servo -{ -struct PIDConfig -{ - // Default values - double dt = 0.001; - double k_p = 1; - double k_i = 0; - double k_d = 0; - double windup_limit = 0.1; -}; - -enum class PoseTrackingStatusCode : int8_t -{ - INVALID = -1, - SUCCESS = 0, - NO_RECENT_TARGET_POSE = 1, - NO_RECENT_END_EFFECTOR_POSE = 2, - STOP_REQUESTED = 3 -}; - -const std::unordered_map POSE_TRACKING_STATUS_CODE_MAP( - { { PoseTrackingStatusCode::INVALID, "Invalid" }, - { PoseTrackingStatusCode::SUCCESS, "Success" }, - { PoseTrackingStatusCode::NO_RECENT_TARGET_POSE, "No recent target pose" }, - { PoseTrackingStatusCode::NO_RECENT_END_EFFECTOR_POSE, "No recent end effector pose" }, - { PoseTrackingStatusCode::STOP_REQUESTED, "Stop requested" } }); - -/** - * Class PoseTracking - subscribe to a target pose. - * Servo toward the target pose. - */ -class PoseTracking -{ -public: - /** \brief Constructor. Loads ROS parameters under the given namespace. */ - PoseTracking(const rclcpp::Node::SharedPtr& node, std::unique_ptr servo_param_listener, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); - - PoseTrackingStatusCode moveToPose(const Eigen::Vector3d& positional_tolerance, const double angular_tolerance, - const double target_pose_timeout); - - /** \brief A method for a different thread to stop motion and return early from control loop */ - void stopMotion(); - - /** \brief Change PID parameters. Motion is stopped before the update */ - void updatePIDConfig(const double x_proportional_gain, const double x_integral_gain, const double x_derivative_gain, - const double y_proportional_gain, const double y_integral_gain, const double y_derivative_gain, - const double z_proportional_gain, const double z_integral_gain, const double z_derivative_gain, - const double angular_proportional_gain, const double angular_integral_gain, - const double angular_derivative_gain); - - void getPIDErrors(double& x_error, double& y_error, double& z_error, double& orientation_error); - - /** - * Get the End Effector link transform. - * The transform from the MoveIt planning frame to EE link - * - * @param transform the transform that will be calculated - * @return true if a valid transform was available - */ - bool getCommandFrameTransform(geometry_msgs::msg::TransformStamped& transform); - - /** \brief Re-initialize the target pose to an empty message. Can be used to reset motion between waypoints. */ - void resetTargetPose(); - - // moveit_servo::Servo instance. Public so we can access member functions like setPaused() - std::unique_ptr servo_; - -private: - /** \brief Load ROS parameters for controller settings. */ - void readROSParams(); - - /** \brief Initialize a PID controller and add it to vector of controllers */ - void initializePID(const PIDConfig& pid_config, std::vector& pid_vector); - - /** \brief Return true if a target pose has been received within timeout [seconds] */ - bool haveRecentTargetPose(const double timeout); - - /** \brief Return true if an end effector pose has been received within timeout [seconds] */ - bool haveRecentEndEffectorPose(const double timeout); - - /** \brief Check if XYZ, roll/pitch/yaw tolerances are satisfied */ - bool satisfiesPoseTolerance(const Eigen::Vector3d& positional_tolerance, const double angular_tolerance); - - /** \brief Subscribe to the target pose on this topic */ - void targetPoseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg); - - /** \brief Update PID controller target positions & orientations */ - void updateControllerSetpoints(); - - /** \brief Update PID controller states (positions & orientations) */ - void updateControllerStateMeasurements(); - - /** \brief Use PID controllers to calculate a full spatial velocity toward a pose */ - geometry_msgs::msg::TwistStamped::ConstSharedPtr calculateTwistCommand(); - - /** \brief Reset flags and PID controllers after a motion completes */ - void doPostMotionReset(); - - rclcpp::Node::SharedPtr node_; - servo::Params servo_parameters_; - - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - moveit::core::RobotModelConstPtr robot_model_; - // Joint group used for controlling the motions - std::string move_group_name_; - - rclcpp::WallRate loop_rate_; - - // ROS interface to Servo - rclcpp::Publisher::SharedPtr twist_stamped_pub_; - - std::vector cartesian_position_pids_; - std::vector cartesian_orientation_pids_; - // Cartesian PID configs - PIDConfig x_pid_config_, y_pid_config_, z_pid_config_, angular_pid_config_; - - // Transforms w.r.t. planning_frame_ - Eigen::Isometry3d command_frame_transform_; - rclcpp::Time command_frame_transform_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); - geometry_msgs::msg::PoseStamped target_pose_; - mutable std::mutex target_pose_mtx_; - - // Subscribe to target pose - rclcpp::Subscription::SharedPtr target_pose_sub_; - - tf2_ros::Buffer transform_buffer_; - tf2_ros::TransformListener transform_listener_; - - // Expected frame name, for error checking and transforms - std::string planning_frame_; - - // Flag that a different thread has requested a stop. - std::atomic stop_requested_; - - std::optional angular_error_; -}; - -// using alias -using PoseTrackingPtr = std::shared_ptr; -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp new file mode 100644 index 0000000000..57caa60f4c --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -0,0 +1,211 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 copyright holder 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 HOLDER 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. + *******************************************************************************/ + +/* Title : servo.hpp + * Project : moveit_servo + * Created : 05/17/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + * + * Description : The core servoing logic. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace moveit_servo +{ + +class Servo +{ +public: + Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr servo_param_listener, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + + ~Servo(); + + // Disable copy construction. + Servo(const Servo&) = delete; + + // Disable copy assignment. + Servo& operator=(Servo&) = delete; + + /** + * \brief Computes the joint state required to follow the given command. + * @param command The command to follow, std::variant type, can handle JointJog, Twist and Pose. + * @return The required joint state. + */ + KinematicState getNextJointState(const ServoInput& command); + + /** + * \brief Set the type of incoming servo command. + * @param command_type The type of command servo should expect. + */ + void setCommandType(const CommandType& command_type); + + /** + * \brief Get the type of command that servo is currently expecting. + * @return The type of command. + */ + CommandType getCommandType(); + + /** + * \brief Get the current status of servo. + * @return The current status. + */ + StatusCode getStatus(); + + /** + * \brief Get the message associated with the current servo status. + * @return The status message. + */ + const std::string getStatusMessage(); + + /** + * \brief Returns the end effector pose in planning frame + */ + const Eigen::Isometry3d getEndEffectorPose(); + + /** + * \brief Start/Stop collision checking thread. + * @param check_collision Stops collision checking thread if false, starts it if true. + */ + void setCollisionChecking(const bool check_collision); + + /** + * \brief Returns the most recent servo parameters. + */ + servo::Params& getParams(); + + /** + * \brief Get the current state of the robot as given by planning scene monitor. + * @return The current state of the robot. + */ + KinematicState getCurrentRobotState(); + + /** + * \brief Smoothly halt at a commanded state when command goes stale. + * @param The last commanded joint states. + * @return The next state stepping towards the required halting state. + */ + std::pair smoothHalt(KinematicState halt_state); + +private: + /** + * \brief Convert a give twist command to planning frame + * (This implementation assumes that source and target frames are stationary) + * See issue: https://github.com/ros-planning/moveit2/issues/2150 + * @param command The twist command to be converted + * @return The transformed twist command. + */ + const TwistCommand toPlanningFrame(const TwistCommand& command); + + /** + * \brief Convert a given pose command to planning frame + * @param command The pose command to be converted + * @return The transformed pose command + */ + const PoseCommand toPlanningFrame(const PoseCommand& command); + + /** + * \brief Compute the change in joint position required to follow the received command. + * @param command The incoming servo command. + * @return The joint position change required (delta). + */ + Eigen::VectorXd jointDeltaFromCommand(const ServoInput& command, moveit::core::RobotStatePtr& robot_state); + + /** + * \brief Updates data depending on joint model group + */ + void updateJointModelGroup(); + + /** + * \brief Validate the servo parameters + * @param servo_params The servo parameters + * @return True if parameters are valid, else False + */ + bool validateParams(const servo::Params& servo_params); + + /** + * \brief Updates the servo parameters and performs validations. + */ + bool updateParams(); + + /** + * \brief Create and initialize the smoothing plugin to be used by servo. + */ + void setSmoothingPlugin(); + + /** + * \brief Apply halting logic to specified joints. + * @param joints_to_halt The indices of joints to be halted. + * @param current_state The current kinematic state. + * @param target_state The target kinematic state. + * @return The bounded kinematic state. + */ + KinematicState haltJoints(const std::vector& joints_to_halt, const KinematicState& current_state, + const KinematicState& target_state); + + // Variables + + StatusCode servo_status_; + // This needs to be threadsafe so it can be updated in realtime. + std::atomic expected_command_type_; + + servo::Params servo_params_; + const rclcpp::Node::SharedPtr node_; + std::shared_ptr servo_param_listener_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + // This value will be updated by CollisionMonitor in a separate thread. + std::atomic collision_velocity_scale_ = 1.0; + std::unique_ptr collision_monitor_; + + pluginlib::UniquePtr smoother_ = nullptr; + + tf2_ros::Buffer transform_buffer_; + tf2_ros::TransformListener transform_listener_; +}; + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h deleted file mode 100644 index 39f780e221..0000000000 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h +++ /dev/null @@ -1,271 +0,0 @@ -/******************************************************************************* - * Title : servo_calcs.h - * Project : moveit_servo - * Created : 1/11/2019 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 copyright holder 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 HOLDER 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. - *******************************************************************************/ - -#pragma once - -// C++ -#include -#include -#include -#include - -// ROS -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// moveit_core -#include - -// moveit_servo -#include -#include -#include - -namespace moveit_servo -{ -enum class ServoType -{ - CARTESIAN_SPACE, - JOINT_SPACE -}; - -class ServoCalcs -{ -public: - ServoCalcs(const rclcpp::Node::SharedPtr& node, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& servo_param_listener); - - ~ServoCalcs(); - - /** - * Start the timer where we do work and publish outputs - * - * @exception can throw a std::runtime_error if the setup was not completed - */ - void start(); - - /** \brief Stop the currently running thread */ - void stop(); - - /** - * Check for parameter update, and apply updates if any - * All dynamic parameters must be checked and updated within this method - */ - void updateParams(); - - /** - * Get the MoveIt planning link transform. - * The transform from the MoveIt planning frame to robot_link_command_frame - * - * @param transform the transform that will be calculated - * @return true if a valid transform was available - */ - bool getCommandFrameTransform(Eigen::Isometry3d& transform); - bool getCommandFrameTransform(geometry_msgs::msg::TransformStamped& transform); - - /** - * Get the End Effector link transform. - * The transform from the MoveIt planning frame to EE link - * - * @param transform the transform that will be calculated - * @return true if a valid transform was available - */ - bool getEEFrameTransform(Eigen::Isometry3d& transform); - bool getEEFrameTransform(geometry_msgs::msg::TransformStamped& transform); - -protected: - /** \brief Run the main calculation loop */ - void mainCalcLoop(); - - /** \brief Do calculations for a single iteration. Publish one outgoing command */ - void calculateSingleIteration(); - - /** \brief Do servoing calculations for Cartesian twist commands. */ - bool cartesianServoCalcs(geometry_msgs::msg::TwistStamped& cmd, - trajectory_msgs::msg::JointTrajectory& joint_trajectory); - - /** \brief Do servoing calculations for direct commands to a joint. */ - bool jointServoCalcs(const control_msgs::msg::JointJog& cmd, trajectory_msgs::msg::JointTrajectory& joint_trajectory); - - /** - * Checks a JointJog msg for valid (non-NaN) velocities - * @param cmd the desired joint servo command - * @return true if this represents a valid joint servo command, false otherwise - */ - bool checkValidCommand(const control_msgs::msg::JointJog& cmd); - - /** - * Checks a TwistStamped msg for valid (non-NaN) velocities - * @param cmd the desired twist servo command - * @return true if this represents a valid servo twist command, false otherwise - */ - bool checkValidCommand(const geometry_msgs::msg::TwistStamped& cmd); - - /** \brief If incoming velocity commands are from a unitless joystick, scale them to physical units. - * Also, multiply by timestep to calculate a position change. - * @return a vector of position deltas - */ - Eigen::VectorXd scaleCartesianCommand(const geometry_msgs::msg::TwistStamped& command); - - /** \brief If incoming velocity commands are from a unitless joystick, scale them to physical units. - * Also, multiply by timestep to calculate a position change. - * @return a vector of position deltas - */ - Eigen::VectorXd scaleJointCommand(const control_msgs::msg::JointJog& command); - - /** \brief Come to a halt in a smooth way. Apply a smoothing plugin, if one is configured. - */ - void filteredHalt(trajectory_msgs::msg::JointTrajectory& joint_trajectory); - - /** \brief Suddenly halt for a joint limit or other critical issue. - * Is handled differently for position vs. velocity control. - */ - void suddenHalt(sensor_msgs::msg::JointState& joint_state, - const std::vector& joints_to_halt) const; - - /** \brief Compose the outgoing JointTrajectory message */ - void composeJointTrajMessage(const sensor_msgs::msg::JointState& joint_state, - trajectory_msgs::msg::JointTrajectory& joint_trajectory); - - /** \brief Set the filters to the specified values */ - void resetLowPassFilters(const sensor_msgs::msg::JointState& joint_state); - - /** \brief Handles all aspects of the servoing after the desired joint commands are established - * Joint and Cartesian calcs feed into here - * Handles limit enforcement, internal state updated, collision scaling, and publishing the commands - * @param delta_theta Eigen vector of joint delta's, from joint or Cartesian servo calcs - * @param joint_trajectory Output trajectory message - * @param servo_type The type of servoing command being used - */ - bool internalServoUpdate(Eigen::ArrayXd& delta_theta, trajectory_msgs::msg::JointTrajectory& joint_trajectory, - const ServoType servo_type); - - /* \brief Command callbacks */ - void twistStampedCB(const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg); - void jointCmdCB(const control_msgs::msg::JointJog::ConstSharedPtr& msg); - void collisionVelocityScaleCB(const std_msgs::msg::Float64::ConstSharedPtr& msg); - - // Pointer to the ROS node - std::shared_ptr node_; - - // Servo parameters - const std::shared_ptr servo_param_listener_; - servo::Params servo_params_; - - // Pointer to the collision environment - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - - // Flag for staying inactive while there are no incoming commands - bool wait_for_servo_commands_ = true; - - // Flag saying if the filters were updated during the timer callback - bool updated_filters_ = false; - - // Incoming command messages - geometry_msgs::msg::TwistStamped twist_stamped_cmd_; - control_msgs::msg::JointJog joint_servo_cmd_; - - const moveit::core::JointModelGroup* joint_model_group_; - - moveit::core::RobotStatePtr current_state_; - - // These variables are mutex protected - // previous_joint_state_ holds the state q(t - dt) - // internal_joint_state_ holds the state q(t) as retrieved from the planning scene monitor. - sensor_msgs::msg::JointState previous_joint_state_, internal_joint_state_; - std::map joint_state_name_map_; - - // Smoothing algorithm (loads a plugin) - pluginlib::UniquePtr smoother_; - - trajectory_msgs::msg::JointTrajectory::SharedPtr last_sent_command_; - - // ROS - rclcpp::Subscription::SharedPtr joint_state_sub_; - rclcpp::Subscription::SharedPtr twist_stamped_sub_; - rclcpp::Subscription::SharedPtr joint_cmd_sub_; - rclcpp::Subscription::SharedPtr collision_velocity_scale_sub_; - rclcpp::Publisher::SharedPtr status_pub_; - rclcpp::Publisher::SharedPtr trajectory_outgoing_cmd_pub_; - rclcpp::Publisher::SharedPtr multiarray_outgoing_cmd_pub_; - - // Main tracking / result publisher loop - std::thread thread_; - bool stop_requested_; - - // Status - StatusCode status_ = StatusCode::NO_WARNING; - bool twist_command_is_stale_ = false; - bool joint_command_is_stale_ = false; - double collision_velocity_scale_ = 1.0; - - // Use ArrayXd type to enable more coefficient-wise operations - Eigen::ArrayXd delta_theta_; - - unsigned int num_joints_; - - // main_loop_mutex_ is used to protect the input state and dynamic parameters - mutable std::mutex main_loop_mutex_; - Eigen::Isometry3d tf_moveit_to_robot_cmd_frame_; - Eigen::Isometry3d tf_moveit_to_ee_frame_; - geometry_msgs::msg::TwistStamped::ConstSharedPtr latest_twist_stamped_; - control_msgs::msg::JointJog::ConstSharedPtr latest_joint_cmd_; - rclcpp::Time latest_twist_command_stamp_ = rclcpp::Time(0., RCL_ROS_TIME); - rclcpp::Time latest_joint_command_stamp_ = rclcpp::Time(0., RCL_ROS_TIME); - - // input condition variable used for low latency mode - std::condition_variable input_cv_; - bool new_input_cmd_ = false; - - // Load a smoothing plugin - pluginlib::ClassLoader smoothing_loader_; - - kinematics::KinematicsBaseConstPtr ik_solver_ = nullptr; -}; -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.h deleted file mode 100644 index 762088b55e..0000000000 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.h +++ /dev/null @@ -1,83 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, 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. - *********************************************************************/ - -/* Title : servo_node.h - * Project : moveit_servo - * Created : 07/13/2020 - * Author : Adam Pettinger - */ - -#pragma once - -#include -#include -#include - -namespace moveit_servo -{ -class ServoNode -{ -public: - ServoNode(const rclcpp::NodeOptions& options); - - // NOLINTNEXTLINE(readability-identifier-naming) - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() - { - return node_->get_node_base_interface(); - } - -private: - std::shared_ptr node_; - std::unique_ptr servo_; - std::shared_ptr tf_buffer_; - std::shared_ptr planning_scene_monitor_; - - /*** - * \brief Most servo parameters are individually validated using the validation methods in - * `generate_parameter_library`, with limits specified in the parameters YAML file. This method performs additional - * validation for parameters whose values depend on each other. - */ - void validateParams(const servo::Params& servo_params); - - /** \brief Start the servo loop. Must be called once to begin Servoing. */ - void startCB(const std::shared_ptr& request, - const std::shared_ptr& response); - rclcpp::Service::SharedPtr start_servo_service_; - - /** \brief Stop the servo loop. */ - void stopCB(const std::shared_ptr& request, - const std::shared_ptr& response); - rclcpp::Service::SharedPtr stop_servo_service_; -}; -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp new file mode 100644 index 0000000000..88bce72f7c --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp @@ -0,0 +1,137 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 copyright holder 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 HOLDER 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. + *******************************************************************************/ + +/* Title : servo_node.hpp + * Project : moveit_servo + * Created : 01/07/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + * + * Description : The ROS API for Servo + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace moveit_servo +{ + +class ServoNode +{ +public: + explicit ServoNode(const rclcpp::NodeOptions& options); + + ~ServoNode(); + + // Disable copy construction. + ServoNode(const ServoNode&) = delete; + + // Disable copy assignment. + ServoNode& operator=(ServoNode&) = delete; + + // This function is required to make this class a valid NodeClass + // see https://docs.ros2.org/latest/api/rclcpp_components/register__node__macro_8hpp.html + // Skip linting due to unconventional function naming + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface(); // NOLINT + +private: + /** + * \brief Loop that handles different types of incoming commands. + */ + void servoLoop(); + + /** + * \brief The service to pause servoing, this does not exit the loop or stop the servo loop thread. + * The loop will be alive even after pausing, but no commands will be processed. + */ + void pauseServo(const std::shared_ptr& request, + const std::shared_ptr& response); + + /** + * \brief The service to set the command type for Servo. + * Supported command types can be found in utils/datatypes.hpp + * This service must be used to set the command type before sending any servoing commands. + */ + void switchCommandType(const std::shared_ptr& request, + const std::shared_ptr& response); + + void jointJogCallback(const control_msgs::msg::JointJog::ConstSharedPtr& msg); + void twistCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg); + void poseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg); + + std::optional processJointJogCommand(); + std::optional processTwistCommand(); + std::optional processPoseCommand(); + + // Variables + + const rclcpp::Node::SharedPtr node_; + std::unique_ptr servo_; + servo::Params servo_params_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + KinematicState last_commanded_state_; // Used when commands go stale; + control_msgs::msg::JointJog latest_joint_jog_; + geometry_msgs::msg::TwistStamped latest_twist_; + geometry_msgs::msg::PoseStamped latest_pose_; + rclcpp::Subscription::SharedPtr joint_jog_subscriber_; + rclcpp::Subscription::SharedPtr twist_subscriber_; + rclcpp::Subscription::SharedPtr pose_subscriber_; + + rclcpp::Publisher::SharedPtr multi_array_publisher_; + rclcpp::Publisher::SharedPtr trajectory_publisher_; + rclcpp::Publisher::SharedPtr status_publisher_; + + rclcpp::Service::SharedPtr switch_command_type_; + rclcpp::Service::SharedPtr pause_servo_; + + // Used for communication with thread + std::atomic stop_servo_; + std::atomic servo_paused_; + std::atomic new_joint_jog_msg_, new_twist_msg_, new_pose_msg_; + + // Threads used by ServoNode + std::thread servo_loop_thread_; +}; + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utilities.h b/moveit_ros/moveit_servo/include/moveit_servo/utilities.h deleted file mode 100644 index 30bedbd061..0000000000 --- a/moveit_ros/moveit_servo/include/moveit_servo/utilities.h +++ /dev/null @@ -1,136 +0,0 @@ -// Copyright 2022 PickNik Inc. -// -// 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 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 HOLDER 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 : Andy Zelenak - Desc : Free functions. We keep them in a separate translation unit to reduce .o filesize - Title : utilities.h - Project : moveit_servo -*/ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace moveit_servo -{ -// Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped -geometry_msgs::msg::TransformStamped convertIsometryToTransform(const Eigen::Isometry3d& eigen_tf, - const std::string& parent_frame, - const std::string& child_frame); - -/** \brief Possibly calculate a velocity scaling factor, due to proximity of - * singularity and direction of motion - * @param[in] joint_model_group The MoveIt group - * @param[in] commanded_twist The commanded Cartesian twist - * @param[in] svd A singular value decomposition of the Jacobian - * @param[in] pseudo_inverse The pseudo-inverse of the Jacobian - * @param[in] hard_stop_singularity_threshold Halt if condition(Jacobian) > hard_stop_singularity_threshold - * @param[in] lower_singularity_threshold Decelerate if condition(Jacobian) > lower_singularity_threshold - * @param[in] leaving_singularity_threshold_multiplier Allow faster motion away from singularity - * @param[in, out] current_state The state of the robot. Used in internal calculations. - * @param[out] status Singularity status - */ -double velocityScalingFactorForSingularity(const moveit::core::JointModelGroup* joint_model_group, - const Eigen::VectorXd& commanded_twist, - const Eigen::JacobiSVD& svd, - const Eigen::MatrixXd& pseudo_inverse, - const double hard_stop_singularity_threshold, - const double lower_singularity_threshold, - const double leaving_singularity_threshold_multiplier, - const moveit::core::RobotStatePtr& current_state, StatusCode& status); - -/** \brief Joint-wise update of a sensor_msgs::msg::JointState with given delta values. - * Also filters and calculates the previous velocity - * @param publish_period The publishing rate for servo command, in seconds. - * @param delta_theta Eigen vector of joint delta values. - * @param previous_joint_state The previous joint state. - * @param current_joint_state The joint state object which will hold the current joint state to send as a command. - * @param smoother The trajectory smoother to be used. - * @return Returns true if successful and false otherwise. - */ -bool applyJointUpdate(const double publish_period, const Eigen::ArrayXd& delta_theta, - const sensor_msgs::msg::JointState& previous_joint_state, - sensor_msgs::msg::JointState& next_joint_state, - pluginlib::UniquePtr& smoother); - -/** \brief Converts a twist command from the command frame to the MoveIt planning frame of the robot - * @param cmd The twist command received from the user - * @param planning_frame Moveit planning frame of the robot - * @param current_state The state of the robot - */ -void transformTwistToPlanningFrame(geometry_msgs::msg::TwistStamped& cmd, const std::string& planning_frame, - const moveit::core::RobotStatePtr& current_state); - -/** \brief Converts the delta_x (change in cartesian position) to a pose to be used with IK solver. - * @param delta_x The change in cartesian position - * @param base_to_tip_frame_transform The transform from base of the robot to its end-effector - * @return Returns the resulting pose after applying delta_x - */ -geometry_msgs::msg::Pose poseFromCartesianDelta(const Eigen::VectorXd& delta_x, - const Eigen::Isometry3d& base_to_tip_frame_transform); - -/** - * @brief Computes the velocity scaling factor based on the joint velocity limits - * @param joint_model_group The MoveIt group - * @param velocity The vector containing the target velocities of the joints - * @return The velocity scaling factor - */ -double getVelocityScalingFactor(const moveit::core::JointModelGroup* joint_model_group, const Eigen::VectorXd& velocity); - -/** - * @brief Decrease robot position change and velocity, if needed, to satisfy joint velocity limits - * @param joint_model_group Active joint group. Used to retrieve limits - * @param publish_period Period of the servo loop - * @param joint_state The command that will go to the robot - * @param override_velocity_scaling_factor Option if the user wants a constant override of the velocity scaling. - * A value greater than 0 will override the internal calculations done by getVelocityScalingFactor - */ -void enforceVelocityLimits(const moveit::core::JointModelGroup* joint_model_group, const double publish_period, - sensor_msgs::msg::JointState& joint_state, - const double override_velocity_scaling_factor = 0.0); - -/** \brief Avoid overshooting joint limits - * @param joint_state The joint state to be checked - * @param joint_limit_margin The allowed margin for joint limit. This is a buffer, prior to the actual limit. - * @param joint_model_group The MoveIt group - * @return Vector of the joints that would move farther past position margin limits - */ -std::vector -enforcePositionLimits(sensor_msgs::msg::JointState& joint_state, const double joint_limit_margin, - const moveit::core::JointModelGroup* joint_model_group); - -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp new file mode 100644 index 0000000000..b012108955 --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/command.hpp @@ -0,0 +1,93 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 copyright holder 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 HOLDER 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. + *******************************************************************************/ + +/* Title : command.hpp + * Project : moveit_servo + * Created : 06/04/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + * + * Description : The methods that compute the required change in joint angles for various input types. + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace moveit_servo +{ + +/** + * \brief Compute the change in joint position for the given joint jog command. + * @param command The joint jog command. + * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor + * @param servo_params The servo parameters + * @return The status and joint position change required (delta). + */ +JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, moveit::core::RobotStatePtr& robot_state, + servo::Params& servo_params); + +/** + * \brief Compute the change in joint position for the given twist command. + * @param command The twist command. + * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor + * @param servo_params The servo parameters + * @return The status and joint position change required (delta). + */ +JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, moveit::core::RobotStatePtr& robot_state, + servo::Params& servo_params); + +/** + * \brief Compute the change in joint position for the given pose command. + * @param command The pose command. + * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor + * @param servo_params The servo parameters + * @return The status and joint position change required (delta). + */ +JointDeltaResult jointDeltaFromPose(const PoseCommand& command, moveit::core::RobotStatePtr& robot_state, + servo::Params& servo_params); + +/** + * \brief Computes the required change in joint angles for given Cartesian change, using the robot's IK solver. + * @param cartesian_position_delta The change in Cartesian position. + * @param robot_state_ The current robot state as obtained from PlanningSceneMonitor + * @param servo_params The servo parameters + * @return The status and joint position change required (delta). + */ +JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delta, + moveit::core::RobotStatePtr& robot_state, servo::Params& servo_params); + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp new file mode 100644 index 0000000000..165d1bae9d --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp @@ -0,0 +1,169 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 copyright holder 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 HOLDER 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. + *******************************************************************************/ + +/* Title : utils.hpp + * Project : moveit_servo + * Created : 05/17/2023 + * Author : Andy Zelenak, V Mohammed Ibrahim + * + * Description : The utility functions used by MoveIt Servo. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace moveit_servo +{ + +/** + * \brief Checks if a given VectorXd is a valid command. + * @param command The command to be checked. + * @return True if the command is valid, else False. + */ +bool isValidCommand(const Eigen::VectorXd& command); + +/** + * \brief Checks if a given Isometry3d (pose) is a valid command. + * @param command The command to be checked. + * @return True if the command is valid, else False. + */ +bool isValidCommand(const Eigen::Isometry3d& command); + +/** + * \brief Checks if a given Twist command is valid. + * @param command The command to be checked. + * @return True if the command is valid, else False. + */ +bool isValidCommand(const TwistCommand& command); + +/** + * \brief Checks if a given Pose command is valid. + * @param command The command to be checked. + * @return True if the command is valid, else False. + */ +bool isValidCommand(const PoseCommand& command); + +/** + * \brief Create a pose message for the provided change in Cartesian position. + * @param delta_x The change in Cartesian position. + * @param base_to_tip_frame_transform The transformation from robot base to ee frame. + * @return The pose message. + */ +geometry_msgs::msg::Pose poseFromCartesianDelta(const Eigen::VectorXd& delta_x, + const Eigen::Isometry3d& base_to_tip_frame_transform); + +/** + * \brief Create a trajectory message from given joint state. + * @param servo_params The configuration used by servo, required for setting some field of the trajectory message. + * @param joint_state The joint state to be added into the trajectory. + * @return The trajectory message. + */ +trajectory_msgs::msg::JointTrajectory composeTrajectoryMessage(const servo::Params& servo_params, + const KinematicState& joint_state); + +/** + * \brief Create a Float64MultiArray message from given joint state + * @param servo_params The configuration used by servo, required for selecting position vs velocity. + * @param joint_state The joint state to be added into the Float64MultiArray. + * @return The Float64MultiArray message. + */ +std_msgs::msg::Float64MultiArray composeMultiArrayMessage(const servo::Params& servo_params, + const KinematicState& joint_state); + +/** + * \brief Computes scaling factor for velocity when the robot is near a singularity. + * @param joint_model_group The joint model group of the robot, used for fetching the Jacobian. + * @param current_state The current state of the robot, used for singularity look ahead. + * @param target_delta_x The vector containing the required change in Cartesian position. + * @param servo_params The servo parameters, contains the singularity thresholds. + * @return The velocity scaling factor and the reason for scaling. + */ +std::pair velocityScalingFactorForSingularity(const moveit::core::RobotStatePtr& robot_state, + const Eigen::VectorXd& target_delta_x, + const servo::Params& servo_params); + +/** + * \brief Apply velocity scaling based on joint limits. + * @param velocities The commanded velocities. + * @param joint_bounds The bounding information for the robot joints. + * @param scaling_override The user defined velocity scaling override. + * @return The velocity scaling factor. + */ +double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities, + const moveit::core::JointBoundsVector& joint_bounds, double scaling_override); + +/** + * \brief Finds the joints that are exceeding allowable position limits. + * @param positions The joint positions. + * @param velocities The current commanded velocities. + * @param joint_bounds The allowable limits for the robot joints. + * @param margin Additional buffer on the actual joint limits. + * @return The joints that are violating the specified position limits. + */ +std::vector jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const moveit::core::JointBoundsVector& joint_bounds, double margin); + +/** + * \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped. + * @param eigen_tf The isometry to be converted to TransformStamped. + * @param parent_frame The target frame. + * @param child_frame The current frame. + * @return The isometry as a TransformStamped message. + */ +geometry_msgs::msg::TransformStamped convertIsometryToTransform(const Eigen::Isometry3d& eigen_tf, + const std::string& parent_frame, + const std::string& child_frame); + +/** + * \brief Convert a PoseStamped message to a Servo Pose + * @param msg The PoseStamped message. + * @return The equivalent Servo Pose type. + */ +PoseCommand poseFromPoseStamped(const geometry_msgs::msg::PoseStamped& msg); + +/** + * \brief Creates the planning scene monitor used by servo + */ +planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, + const servo::Params& servo_params); +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h b/moveit_ros/moveit_servo/include/moveit_servo/utils/datatypes.hpp similarity index 58% rename from moveit_ros/moveit_servo/include/moveit_servo/status_codes.h rename to moveit_ros/moveit_servo/include/moveit_servo/utils/datatypes.hpp index f5942fa4be..31195c37f2 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils/datatypes.hpp @@ -1,9 +1,4 @@ /******************************************************************************* - * Title : status_codes.h - * Project : moveit_servo - * Created : 2/25/2019 - * Author : Andy Zelenak - * * BSD 3-Clause License * * Copyright (c) 2019, Los Alamos National Security, LLC @@ -36,24 +31,33 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ +/* Title : datatypes.hpp + * Project : moveit_servo + * Created : 06/05/2023 + * Author : Andy Zelenak, V Mohammed Ibrahim + * + * Description : The custom datatypes used by Moveit Servo. + */ + #pragma once #include +#include #include namespace moveit_servo { + enum class StatusCode : int8_t { INVALID = -1, NO_WARNING = 0, DECELERATE_FOR_APPROACHING_SINGULARITY = 1, HALT_FOR_SINGULARITY = 2, - DECELERATE_FOR_COLLISION = 3, - HALT_FOR_COLLISION = 4, - JOINT_BOUND = 5, - DECELERATE_FOR_LEAVING_SINGULARITY = 6, - PAUSED = 7 + DECELERATE_FOR_LEAVING_SINGULARITY = 3, + DECELERATE_FOR_COLLISION = 4, + HALT_FOR_COLLISION = 5, + JOINT_BOUND = 6 }; const std::unordered_map SERVO_STATUS_CODE_MAP( @@ -61,9 +65,68 @@ const std::unordered_map SERVO_STATUS_CODE_MAP( { StatusCode::NO_WARNING, "No warnings" }, { StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY, "Moving closer to a singularity, decelerating" }, { StatusCode::HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" }, + { StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY, "Moving away from a singularity, decelerating" }, { StatusCode::DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" }, { StatusCode::HALT_FOR_COLLISION, "Collision detected, emergency stop" }, - { StatusCode::JOINT_BOUND, "Close to a joint bound (position or velocity), halting" }, - { StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY, "Moving away from a singularity, decelerating" }, - { StatusCode::PAUSED, "Paused" } }); + { StatusCode::JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } }); + +// The datatype that specifies the type of command that servo should expect. +enum class CommandType : int8_t +{ + JOINT_JOG = 0, + TWIST = 1, + POSE = 2, + + // Range of allowed values used for validation. + MIN = JOINT_JOG, + MAX = POSE +}; + +typedef std::pair JointDeltaResult; + +// The joint jog command, this will be vector of length equal to the number of joints of the robot. +struct JointJogCommand +{ + std::vector names; + std::vector velocities; +}; + +// The twist command, frame_id is the name of the frame in which the command is specified in. +// frame_id must always be specified. +struct TwistCommand +{ + std::string frame_id; + Eigen::Vector velocities; +}; + +// The Pose command, frame_id is the name of the frame in which the command is specified in. +// frame_id must always be specified. +struct PoseCommand +{ + std::string frame_id; + Eigen::Isometry3d pose; +}; + +// The generic input type for servo that can be JointJog, Twist or Pose. +typedef std::variant ServoInput; + +// The output datatype of servo, this structure contains the names of the joints along with their positions, velocities and accelerations. +struct KinematicState +{ + std::vector joint_names; + std::vector positions, velocities, accelerations; + + KinematicState(const int num_joints) + { + joint_names.resize(num_joints); + positions.resize(num_joints); + velocities.resize(num_joints); + accelerations.resize(num_joints); + } + + KinematicState() + { + } +}; + } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/launch/demo_joint_jog.launch.py b/moveit_ros/moveit_servo/launch/demo_joint_jog.launch.py new file mode 100644 index 0000000000..6167c23083 --- /dev/null +++ b/moveit_ros/moveit_servo/launch/demo_joint_jog.launch.py @@ -0,0 +1,119 @@ +import os +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory +from launch_param_builder import ParameterBuilder +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() + ) + + # Get parameters for the Servo node + servo_params = { + "moveit_servo": ParameterBuilder("moveit_servo") + .yaml("config/panda_simulated_config.yaml") + .to_dict() + } + + # This filter parameter should be >1. Increase it for greater smoothing but slower motion. + low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + + # RViz + rviz_config_file = ( + get_package_share_directory("moveit_servo") + "/config/demo_rviz_config.rviz" + ) + rviz_node = launch_ros.actions.Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + ], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="screen", + ) + + joint_state_broadcaster_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager-timeout", + "300", + "--controller-manager", + "/controller_manager", + ], + ) + + panda_arm_controller_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + # Launch as much as possible in components + container = launch_ros.actions.ComposableNodeContainer( + name="moveit_servo_demo_container", + namespace="/", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + launch_ros.descriptions.ComposableNode( + package="robot_state_publisher", + plugin="robot_state_publisher::RobotStatePublisher", + name="robot_state_publisher", + parameters=[moveit_config.robot_description], + ), + launch_ros.descriptions.ComposableNode( + package="tf2_ros", + plugin="tf2_ros::StaticTransformBroadcasterNode", + name="static_tf2_broadcaster", + parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}], + ), + ], + output="screen", + ) + # Launch a standalone Servo node. + # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC + servo_node = launch_ros.actions.Node( + package="moveit_servo", + executable="demo_joint_jog", + parameters=[ + servo_params, + low_pass_filter_coeff, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + output="screen", + ) + + return launch.LaunchDescription( + [ + rviz_node, + ros2_control_node, + joint_state_broadcaster_spawner, + panda_arm_controller_spawner, + servo_node, + container, + ] + ) diff --git a/moveit_ros/moveit_servo/launch/demo_pose.launch.py b/moveit_ros/moveit_servo/launch/demo_pose.launch.py new file mode 100644 index 0000000000..674c40b9da --- /dev/null +++ b/moveit_ros/moveit_servo/launch/demo_pose.launch.py @@ -0,0 +1,119 @@ +import os +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory +from launch_param_builder import ParameterBuilder +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() + ) + + # Get parameters for the Servo node + servo_params = { + "moveit_servo": ParameterBuilder("moveit_servo") + .yaml("config/panda_simulated_config.yaml") + .to_dict() + } + + # This filter parameter should be >1. Increase it for greater smoothing but slower motion. + low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + + # RViz + rviz_config_file = ( + get_package_share_directory("moveit_servo") + "/config/demo_rviz_config.rviz" + ) + rviz_node = launch_ros.actions.Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + ], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="screen", + ) + + joint_state_broadcaster_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager-timeout", + "300", + "--controller-manager", + "/controller_manager", + ], + ) + + panda_arm_controller_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + # Launch as much as possible in components + container = launch_ros.actions.ComposableNodeContainer( + name="moveit_servo_demo_container", + namespace="/", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + launch_ros.descriptions.ComposableNode( + package="robot_state_publisher", + plugin="robot_state_publisher::RobotStatePublisher", + name="robot_state_publisher", + parameters=[moveit_config.robot_description], + ), + launch_ros.descriptions.ComposableNode( + package="tf2_ros", + plugin="tf2_ros::StaticTransformBroadcasterNode", + name="static_tf2_broadcaster", + parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}], + ), + ], + output="screen", + ) + # Launch a standalone Servo node. + # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC + servo_node = launch_ros.actions.Node( + package="moveit_servo", + executable="demo_pose", + parameters=[ + servo_params, + low_pass_filter_coeff, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + output="screen", + ) + + return launch.LaunchDescription( + [ + rviz_node, + ros2_control_node, + joint_state_broadcaster_spawner, + panda_arm_controller_spawner, + servo_node, + container, + ] + ) diff --git a/moveit_ros/moveit_servo/launch/servo_example.launch.py b/moveit_ros/moveit_servo/launch/demo_ros_api.launch.py similarity index 89% rename from moveit_ros/moveit_servo/launch/servo_example.launch.py rename to moveit_ros/moveit_servo/launch/demo_ros_api.launch.py index 224cb6bf55..41ba40d3bc 100644 --- a/moveit_ros/moveit_servo/launch/servo_example.launch.py +++ b/moveit_ros/moveit_servo/launch/demo_ros_api.launch.py @@ -9,17 +9,17 @@ def generate_launch_description(): - # Launch Servo as a standalone node or as a "node component" for better latency/efficiency - launch_as_standalone_node = LaunchConfiguration( - "launch_as_standalone_node", default="false" - ) - moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") .to_moveit_configs() ) + # Launch Servo as a standalone node or as a "node component" for better latency/efficiency + launch_as_standalone_node = LaunchConfiguration( + "launch_as_standalone_node", default="false" + ) + # Get parameters for the Servo node servo_params = { "moveit_servo": ParameterBuilder("moveit_servo") @@ -32,7 +32,8 @@ def generate_launch_description(): # RViz rviz_config_file = ( - get_package_share_directory("moveit_servo") + "/config/demo_rviz_config.rviz" + get_package_share_directory("moveit_servo") + + "/config/demo_rviz_config_ros.rviz" ) rviz_node = launch_ros.actions.Node( package="rviz2", @@ -92,8 +93,10 @@ def generate_launch_description(): name="servo_node", parameters=[ servo_params, + low_pass_filter_coeff, moveit_config.robot_description, moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, ], condition=UnlessCondition(launch_as_standalone_node), ), @@ -109,25 +112,15 @@ def generate_launch_description(): name="static_tf2_broadcaster", parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}], ), - launch_ros.descriptions.ComposableNode( - package="moveit_servo", - plugin="moveit_servo::JoyToServoPub", - name="controller_to_servo_node", - ), - launch_ros.descriptions.ComposableNode( - package="joy", - plugin="joy::Joy", - name="joy_node", - ), ], output="screen", ) - - # Optionally launch a standalone Servo node. + # Launch a standalone Servo node. # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC servo_node = launch_ros.actions.Node( package="moveit_servo", - executable="servo_node_main", + executable="servo_node", + name="servo_node", parameters=[ servo_params, low_pass_filter_coeff, diff --git a/moveit_ros/moveit_servo/launch/demo_twist.launch.py b/moveit_ros/moveit_servo/launch/demo_twist.launch.py new file mode 100644 index 0000000000..63bd829c3b --- /dev/null +++ b/moveit_ros/moveit_servo/launch/demo_twist.launch.py @@ -0,0 +1,119 @@ +import os +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory +from launch_param_builder import ParameterBuilder +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() + ) + + # Get parameters for the Servo node + servo_params = { + "moveit_servo": ParameterBuilder("moveit_servo") + .yaml("config/panda_simulated_config.yaml") + .to_dict() + } + + # This filter parameter should be >1. Increase it for greater smoothing but slower motion. + low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + + # RViz + rviz_config_file = ( + get_package_share_directory("moveit_servo") + "/config/demo_rviz_config.rviz" + ) + rviz_node = launch_ros.actions.Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + ], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="screen", + ) + + joint_state_broadcaster_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager-timeout", + "300", + "--controller-manager", + "/controller_manager", + ], + ) + + panda_arm_controller_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + # Launch as much as possible in components + container = launch_ros.actions.ComposableNodeContainer( + name="moveit_servo_demo_container", + namespace="/", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + launch_ros.descriptions.ComposableNode( + package="robot_state_publisher", + plugin="robot_state_publisher::RobotStatePublisher", + name="robot_state_publisher", + parameters=[moveit_config.robot_description], + ), + launch_ros.descriptions.ComposableNode( + package="tf2_ros", + plugin="tf2_ros::StaticTransformBroadcasterNode", + name="static_tf2_broadcaster", + parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}], + ), + ], + output="screen", + ) + # Launch a standalone Servo node. + # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC + servo_node = launch_ros.actions.Node( + package="moveit_servo", + executable="demo_twist", + parameters=[ + servo_params, + low_pass_filter_coeff, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + output="screen", + ) + + return launch.LaunchDescription( + [ + rviz_node, + ros2_control_node, + joint_state_broadcaster_spawner, + panda_arm_controller_spawner, + servo_node, + container, + ] + ) diff --git a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py deleted file mode 100644 index aac0882bab..0000000000 --- a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py +++ /dev/null @@ -1,110 +0,0 @@ -import os -from launch import LaunchDescription -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory -from moveit_configs_utils import MoveItConfigsBuilder -from launch_param_builder import ParameterBuilder - - -def generate_launch_description(): - moveit_config = ( - MoveItConfigsBuilder("moveit_resources_panda") - .robot_description(file_path="config/panda.urdf.xacro") - .to_moveit_configs() - ) - - # Get parameters for the Pose Tracking node - pose_tracker_params = { - "moveit_servo": ParameterBuilder("moveit_servo") - .yaml("config/pose_tracking_settings.yaml") - .to_dict() - } - - servo_params = { - "moveit_servo": ParameterBuilder("moveit_servo") - .yaml("config/panda_simulated_config_pose_tracking.yaml") - .to_dict() - } - - # RViz - rviz_config_file = ( - get_package_share_directory("moveit_servo") - + "/config/demo_rviz_pose_tracking.rviz" - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - # prefix=['xterm -e gdb -ex run --args'], - output="log", - arguments=["-d", rviz_config_file], - parameters=[moveit_config.to_dict()], - ) - - # Publishes tf's for the robot - robot_state_publisher = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="screen", - parameters=[moveit_config.robot_description], - ) - - # A node to publish world -> panda_link0 transform - static_tf = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_transform_publisher", - output="log", - arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], - ) - - pose_tracking_node = Node( - package="moveit_servo", - executable="servo_pose_tracking_demo", - # prefix=['xterm -e gdb -ex run --args'], - output="screen", - parameters=[moveit_config.to_dict(), servo_params, pose_tracker_params], - ) - - # ros2_control using FakeSystem as hardware - ros2_controllers_path = os.path.join( - get_package_share_directory("moveit_resources_panda_moveit_config"), - "config", - "ros2_controllers.yaml", - ) - ros2_control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[moveit_config.robot_description, ros2_controllers_path], - output="screen", - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "joint_state_broadcaster", - "--controller-manager-timeout", - "300", - "--controller-manager", - "/controller_manager", - ], - ) - - panda_arm_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["panda_arm_controller", "-c", "/controller_manager"], - ) - - return LaunchDescription( - [ - rviz_node, - static_tf, - pose_tracking_node, - ros2_control_node, - joint_state_broadcaster_spawner, - panda_arm_controller_spawner, - robot_state_publisher, - ] - ) diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index fa895b0117..6f7d667d5e 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -24,7 +24,6 @@ moveit_common control_msgs - control_toolbox generate_parameter_library geometry_msgs moveit_msgs diff --git a/moveit_ros/moveit_servo/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp deleted file mode 100644 index 3a3c8ede94..0000000000 --- a/moveit_ros/moveit_servo/src/collision_check.cpp +++ /dev/null @@ -1,173 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 copyright holder 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 HOLDER 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. - *******************************************************************************/ - -/* Title : collision_check.cpp - * Project : moveit_servo - * Created : 1/11/2019 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - */ - -#include - -#include -// #include - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.collision_check"); -static const double MIN_RECOMMENDED_COLLISION_RATE = 10; -constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30 * 1000; // Milliseconds to throttle logs inside loops - -namespace moveit_servo -{ -// Constructor for the class that handles collision checking -CollisionCheck::CollisionCheck(const rclcpp::Node::SharedPtr& node, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& servo_param_listener) - : node_(node) - , servo_param_listener_(servo_param_listener) - , servo_params_(servo_param_listener_->get_params()) - , planning_scene_monitor_(planning_scene_monitor) - , self_velocity_scale_coefficient_(-log(0.001) / servo_params_.self_collision_proximity_threshold) - , scene_velocity_scale_coefficient_(-log(0.001) / servo_params_.scene_collision_proximity_threshold) - , period_(1.0 / servo_params_.collision_check_rate) -{ - // Init collision request - collision_request_.group_name = servo_params_.move_group_name; - collision_request_.distance = true; // enable distance-based collision checking - collision_request_.contacts = true; // Record the names of collision pairs - - if (servo_params_.collision_check_rate < MIN_RECOMMENDED_COLLISION_RATE) - { - auto& clk = *node_->get_clock(); -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clk, ROS_LOG_THROTTLE_PERIOD, - "Collision check rate is low, increase it in yaml file if CPU allows"); -#pragma GCC diagnostic pop - } - - // ROS pubs/subs - collision_velocity_scale_pub_ = - node_->create_publisher("~/collision_velocity_scale", rclcpp::SystemDefaultsQoS()); - - current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); -} - -planning_scene_monitor::LockedPlanningSceneRO CollisionCheck::getLockedPlanningSceneRO() const -{ - return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); -} - -void CollisionCheck::start() -{ - timer_ = node_->create_wall_timer(std::chrono::duration(period_), [this]() { return run(); }); -} - -void CollisionCheck::stop() -{ - if (timer_) - { - timer_->cancel(); - } -} - -void CollisionCheck::run() -{ - // Update the latest parameters - if (servo_params_.enable_parameter_update) - { - servo_params_ = servo_param_listener_->get_params(); - } - - // Only do collision checking if the check_collisions parameter is currently true. - velocity_scale_ = 1; - if (servo_params_.check_collisions) - { - // Update to the latest current state - current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); - current_state_->updateCollisionBodyTransforms(); - collision_detected_ = false; - - // Do a timer-safe distance-based collision detection - collision_result_.clear(); - getLockedPlanningSceneRO()->getCollisionEnv()->checkRobotCollision(collision_request_, collision_result_, - *current_state_); - scene_collision_distance_ = collision_result_.distance; - collision_detected_ |= collision_result_.collision; - collision_result_.print(); - - collision_result_.clear(); - // Self-collisions and scene collisions are checked separately so different thresholds can be used - getLockedPlanningSceneRO()->getCollisionEnvUnpadded()->checkSelfCollision( - collision_request_, collision_result_, *current_state_, getLockedPlanningSceneRO()->getAllowedCollisionMatrix()); - self_collision_distance_ = collision_result_.distance; - collision_detected_ |= collision_result_.collision; - collision_result_.print(); - - // If we're definitely in collision, stop immediately - if (collision_detected_) - { - velocity_scale_ = 0; - } - else - { - // If we are far from a collision, velocity_scale should be 1. - // If we are very close to a collision, velocity_scale should be ~zero. - // When scene_collision_proximity_threshold is breached, start decelerating exponentially. - if (scene_collision_distance_ < servo_params_.scene_collision_proximity_threshold) - { - // velocity_scale = e ^ k * (collision_distance - threshold) - // k = - ln(0.001) / collision_proximity_threshold - // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. - // velocity_scale should equal 0.001 when collision_distance is at zero. - velocity_scale_ = std::min( - velocity_scale_, exp(scene_velocity_scale_coefficient_ * - (scene_collision_distance_ - servo_params_.scene_collision_proximity_threshold))); - } - - if (self_collision_distance_ < servo_params_.self_collision_proximity_threshold) - { - velocity_scale_ = std::min(velocity_scale_, - exp(self_velocity_scale_coefficient_ * - (self_collision_distance_ - servo_params_.self_collision_proximity_threshold))); - } - } - } - - // Publish collision velocity scaling message. - { - auto msg = std::make_unique(); - msg->data = velocity_scale_; - collision_velocity_scale_pub_->publish(std::move(msg)); - } -} -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/collision_monitor.cpp b/moveit_ros/moveit_servo/src/collision_monitor.cpp new file mode 100644 index 0000000000..2ec810840b --- /dev/null +++ b/moveit_ros/moveit_servo/src/collision_monitor.cpp @@ -0,0 +1,157 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 copyright holder 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 HOLDER 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. + *******************************************************************************/ +/* + * Title : collision_monitor.cpp + * Project : moveit_servo + * Created : 06/08/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + */ + +#include +#include + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo"); +} + +namespace moveit_servo +{ + +CollisionMonitor::CollisionMonitor(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const servo::Params& servo_params, std::atomic& collision_velocity_scale) + : servo_params_(servo_params) + , planning_scene_monitor_(planning_scene_monitor) + , collision_velocity_scale_(collision_velocity_scale) +{ +} + +void CollisionMonitor::start() +{ + stop_requested_ = false; + if (!monitor_thread_.joinable()) + { + monitor_thread_ = std::thread(&CollisionMonitor::checkCollisions, this); + RCLCPP_INFO_STREAM(LOGGER, "Collision monitor started"); + } + else + { + RCLCPP_INFO_STREAM(LOGGER, "Collision monitor could not be started"); + } +} + +void CollisionMonitor::stop() +{ + stop_requested_ = true; + if (monitor_thread_.joinable()) + { + monitor_thread_.join(); + } + RCLCPP_INFO_STREAM(LOGGER, "Collision monitor stopped"); +} + +void CollisionMonitor::checkCollisions() +{ + rclcpp::WallRate rate(servo_params_.collision_check_rate); + + bool approaching_self_collision, approaching_scene_collision; + double self_collision_threshold_delta, scene_collision_threshold_delta; + double self_collision_scale, scene_collision_scale; + const double self_velocity_scale_coefficient{ -log(0.001) / servo_params_.self_collision_proximity_threshold }; + const double scene_velocity_scale_coefficient{ -log(0.001) / servo_params_.scene_collision_proximity_threshold }; + + while (rclcpp::ok() && !stop_requested_) + { + // Reset the scale on every iteration. + collision_velocity_scale_ = 1.0; + + if (servo_params_.check_collisions) + { + // Fetch latest robot state. + robot_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + // This must be called before doing collision checking. + robot_state_->updateCollisionBodyTransforms(); + + // Get a read-only copy of planning scene. + planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_); + + // Check collision with environment. + scene_collision_result_.clear(); + locked_scene->getCollisionEnv()->checkRobotCollision(scene_collision_request_, scene_collision_result_, + *robot_state_); + + // Check robot self collision. + self_collision_result_.clear(); + locked_scene->getCollisionEnvUnpadded()->checkSelfCollision( + self_collision_request_, self_collision_result_, *robot_state_, locked_scene->getAllowedCollisionMatrix()); + + // If collision detected scale velocity to 0, else start decelerating exponentially. + // velocity_scale = e ^ k * (collision_distance - threshold) + // k = - ln(0.001) / collision_proximity_threshold + // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. + // velocity_scale should equal 0.001 when collision_distance is at zero. + + if (self_collision_result_.collision || scene_collision_result_.collision) + { + collision_velocity_scale_ = 0.0; + } + else + { + self_collision_scale = scene_collision_scale = 1.0; + + approaching_scene_collision = + scene_collision_result_.distance < servo_params_.scene_collision_proximity_threshold; + approaching_self_collision = self_collision_result_.distance < servo_params_.self_collision_proximity_threshold; + + if (approaching_scene_collision) + { + scene_collision_threshold_delta = + scene_collision_result_.distance - servo_params_.scene_collision_proximity_threshold; + scene_collision_scale = std::exp(scene_velocity_scale_coefficient * scene_collision_threshold_delta); + } + + if (approaching_self_collision) + { + self_collision_threshold_delta = + self_collision_result_.distance - servo_params_.self_collision_proximity_threshold; + self_collision_scale = std::exp(self_velocity_scale_coefficient * self_collision_threshold_delta); + } + + // Use the scaling factor with lower value, i.e maximum scale down. + collision_velocity_scale_ = std::min(scene_collision_scale, self_collision_scale); + } + } + rate.sleep(); + } +} +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/cpp_interface_demo/pose_tracking_demo.cpp b/moveit_ros/moveit_servo/src/cpp_interface_demo/pose_tracking_demo.cpp deleted file mode 100644 index 2f52cf1657..0000000000 --- a/moveit_ros/moveit_servo/src/cpp_interface_demo/pose_tracking_demo.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/******************************************************************************* - * Title : pose_tracking_example.cpp - * Project : moveit_servo - * Created : 09/04/2020 - * Author : Adam Pettinger - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 copyright holder 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 HOLDER 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. - *******************************************************************************/ - -#include -#include - -#include -#include -#include -#include -#include - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.pose_tracking_demo"); - -// Class for monitoring status of moveit_servo -class StatusMonitor -{ -public: - StatusMonitor(const rclcpp::Node::SharedPtr& node, const std::string& topic) - { - sub_ = node->create_subscription(topic, rclcpp::SystemDefaultsQoS(), - [this](const std_msgs::msg::Int8::ConstSharedPtr& msg) { - return statusCB(msg); - }); - } - -private: - void statusCB(const std_msgs::msg::Int8::ConstSharedPtr& msg) - { - moveit_servo::StatusCode latest_status = static_cast(msg->data); - if (latest_status != status_) - { - status_ = latest_status; - const auto& status_str = moveit_servo::SERVO_STATUS_CODE_MAP.at(status_); - RCLCPP_INFO_STREAM(LOGGER, "Servo status: " << status_str); - } - } - - moveit_servo::StatusCode status_ = moveit_servo::StatusCode::INVALID; - rclcpp::Subscription::SharedPtr sub_; -}; - -/** - * Instantiate the pose tracking interface. - * Send a pose slightly different from the starting pose - * Then keep updating the target pose a little bit - */ -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("pose_tracking_demo"); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); - std::thread executor_thread([&executor]() { executor.spin(); }); - - auto servo_param_listener = std::make_unique(node, "moveit_servo"); - auto servo_parameters = servo_param_listener->get_params(); - - // Load the planning scene monitor - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor; - planning_scene_monitor = std::make_shared(node, "robot_description"); - if (!planning_scene_monitor->getPlanningScene()) - { - RCLCPP_ERROR_STREAM(LOGGER, "Error in setting up the PlanningSceneMonitor."); - exit(EXIT_FAILURE); - } - - planning_scene_monitor->providePlanningSceneService(); - planning_scene_monitor->startSceneMonitor(); - planning_scene_monitor->startWorldGeometryMonitor( - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, - false /* skip octomap monitor */); - planning_scene_monitor->startStateMonitor(servo_parameters.joint_topic); - planning_scene_monitor->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE); - - // Wait for Planning Scene Monitor to setup - if (!planning_scene_monitor->waitForCurrentRobotState(node->now(), 5.0 /* seconds */)) - { - RCLCPP_ERROR_STREAM(LOGGER, "Error waiting for current robot state in PlanningSceneMonitor."); - exit(EXIT_FAILURE); - } - - // Create the pose tracker - moveit_servo::PoseTracking tracker(node, std::move(servo_param_listener), planning_scene_monitor); - - // Make a publisher for sending pose commands - auto target_pose_pub = - node->create_publisher("target_pose", rclcpp::SystemDefaultsQoS()); - - // Subscribe to servo status (and log it when it changes) - StatusMonitor status_monitor(node, servo_parameters.status_topic); - - Eigen::Vector3d lin_tol{ 0.001, 0.001, 0.001 }; - double rot_tol = 0.01; - - // Get the current EE transform - geometry_msgs::msg::TransformStamped current_ee_tf; - tracker.getCommandFrameTransform(current_ee_tf); - - // Convert it to a Pose - geometry_msgs::msg::PoseStamped target_pose; - target_pose.header.frame_id = current_ee_tf.header.frame_id; - target_pose.pose.position.x = current_ee_tf.transform.translation.x; - target_pose.pose.position.y = current_ee_tf.transform.translation.y; - target_pose.pose.position.z = current_ee_tf.transform.translation.z; - target_pose.pose.orientation = current_ee_tf.transform.rotation; - - // Modify it a little bit - target_pose.pose.position.x += 0.1; - - // resetTargetPose() can be used to clear the target pose and wait for a new one, e.g. when moving between multiple - // waypoints - tracker.resetTargetPose(); - - // Publish target pose - target_pose.header.stamp = node->now(); - target_pose_pub->publish(target_pose); - - // Run the pose tracking in a new thread - std::thread move_to_pose_thread([&tracker, &lin_tol, &rot_tol] { - moveit_servo::PoseTrackingStatusCode tracking_status = - tracker.moveToPose(lin_tol, rot_tol, 0.1 /* target pose timeout */); - RCLCPP_INFO_STREAM(LOGGER, "Pose tracker exited with status: " - << moveit_servo::POSE_TRACKING_STATUS_CODE_MAP.at(tracking_status)); - }); - - rclcpp::WallRate loop_rate(50); - for (size_t i = 0; i < 500; ++i) - { - // Modify the pose target a little bit each cycle - // This is a dynamic pose target - target_pose.pose.position.z += 0.0004; - target_pose.header.stamp = node->now(); - target_pose_pub->publish(target_pose); - - loop_rate.sleep(); - } - - // Make sure the tracker is stopped and clean up - move_to_pose_thread.join(); - - // Kill executor thread before shutdown - executor.cancel(); - executor_thread.join(); - - rclcpp::shutdown(); - return EXIT_SUCCESS; -} diff --git a/moveit_ros/moveit_servo/src/pose_tracking.cpp b/moveit_ros/moveit_servo/src/pose_tracking.cpp deleted file mode 100644 index 359f202523..0000000000 --- a/moveit_ros/moveit_servo/src/pose_tracking.cpp +++ /dev/null @@ -1,405 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, 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. - *********************************************************************/ - -#include - -#include -using namespace std::literals; - -namespace -{ -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.pose_tracking"); -constexpr size_t LOG_THROTTLE_PERIOD = 10; // sec - -// Helper template for declaring and getting ros param -template -void declareOrGetParam(T& output_value, const std::string& param_name, const rclcpp::Node::SharedPtr& node, - const rclcpp::Logger& logger, const T default_value = T{}) -{ - try - { - if (node->has_parameter(param_name)) - { - node->get_parameter(param_name, output_value); - } - else - { - output_value = node->declare_parameter(param_name, default_value); - } - } - catch (const rclcpp::exceptions::InvalidParameterTypeException& e) - { - RCLCPP_WARN_STREAM(logger, "InvalidParameterTypeException(" << param_name << "): " << e.what()); - RCLCPP_ERROR_STREAM(logger, "Error getting parameter \'" << param_name << "\', check parameter type in YAML file"); - throw e; - } - - RCLCPP_INFO_STREAM(logger, "Found parameter - " << param_name << ": " << output_value); -} -} // namespace - -namespace moveit_servo -{ -PoseTracking::PoseTracking(const rclcpp::Node::SharedPtr& node, - std::unique_ptr servo_param_listener, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) - : node_(node) - , servo_parameters_(servo_param_listener->get_params()) - , planning_scene_monitor_(planning_scene_monitor) - , loop_rate_(1.0 / servo_parameters_.publish_period) - , transform_buffer_(node_->get_clock()) - , transform_listener_(transform_buffer_) - , stop_requested_(false) -{ - readROSParams(); - robot_model_ = planning_scene_monitor_->getRobotModel(); - - // Initialize PID controllers - initializePID(x_pid_config_, cartesian_position_pids_); - initializePID(y_pid_config_, cartesian_position_pids_); - initializePID(z_pid_config_, cartesian_position_pids_); - initializePID(angular_pid_config_, cartesian_orientation_pids_); - - // Connect to Servo ROS interfaces - target_pose_sub_ = node_->create_subscription( - "target_pose", rclcpp::SystemDefaultsQoS(), - [this](const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg) { return targetPoseCallback(msg); }); - - // Publish outgoing twist commands to the Servo object - twist_stamped_pub_ = node_->create_publisher( - servo_parameters_.cartesian_command_in_topic, rclcpp::SystemDefaultsQoS()); - - // Use the C++ interface that Servo provides - servo_ = std::make_unique(node_, planning_scene_monitor_, std::move(servo_param_listener)); - servo_->start(); -} - -PoseTrackingStatusCode PoseTracking::moveToPose(const Eigen::Vector3d& positional_tolerance, - const double angular_tolerance, const double target_pose_timeout) -{ - // Reset stop requested flag before starting motions - stop_requested_ = false; - // Wait a bit for a target pose message to arrive. - // The target pose may get updated by new messages as the robot moves (in a callback function). - const rclcpp::Time start_time = node_->now(); - - while ((!haveRecentTargetPose(target_pose_timeout) || !haveRecentEndEffectorPose(target_pose_timeout)) && - ((node_->now() - start_time).seconds() < target_pose_timeout)) - { - if (servo_->getCommandFrameTransform(command_frame_transform_)) - { - command_frame_transform_stamp_ = node_->now(); - } - std::this_thread::sleep_for(1ms); - } - - if (!haveRecentTargetPose(target_pose_timeout)) - { - RCLCPP_ERROR_STREAM(LOGGER, "The target pose was not updated recently. Aborting."); - return PoseTrackingStatusCode::NO_RECENT_TARGET_POSE; - } - - // Continue sending PID controller output to Servo until one of the following conditions is met: - // - Goal tolerance is satisfied - // - Target pose becomes outdated - // - Command frame transform becomes outdated - // - Another thread requested a stop - while (rclcpp::ok()) - { - if (satisfiesPoseTolerance(positional_tolerance, angular_tolerance)) - { - RCLCPP_INFO_STREAM(LOGGER, "The target pose is achieved!"); - break; - } - // Attempt to update robot pose - if (servo_->getCommandFrameTransform(command_frame_transform_)) - { - command_frame_transform_stamp_ = node_->now(); - } - - // Check that end-effector pose (command frame transform) is recent enough. - if (!haveRecentEndEffectorPose(target_pose_timeout)) - { - RCLCPP_ERROR_STREAM(LOGGER, "The end effector pose was not updated in time. Aborting."); - doPostMotionReset(); - return PoseTrackingStatusCode::NO_RECENT_END_EFFECTOR_POSE; - } - - if (stop_requested_) - { - RCLCPP_INFO_STREAM(LOGGER, "Halting servo motion, a stop was requested."); - doPostMotionReset(); - return PoseTrackingStatusCode::STOP_REQUESTED; - } - - // Compute servo command from PID controller output and send it to the Servo object, for execution - twist_stamped_pub_->publish(*calculateTwistCommand()); - - if (!loop_rate_.sleep()) - { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wold-style-cast" - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, *node_->get_clock(), LOG_THROTTLE_PERIOD, "Target control rate was missed"); -#pragma GCC diagnostic pop - } - } - - doPostMotionReset(); - return PoseTrackingStatusCode::SUCCESS; -} - -void PoseTracking::readROSParams() -{ - planning_frame_ = servo_parameters_.planning_frame; - move_group_name_ = servo_parameters_.move_group_name; - - if (!planning_scene_monitor_->getRobotModel()->hasJointModelGroup(move_group_name_)) - { - RCLCPP_ERROR_STREAM(LOGGER, "Unable to find the specified joint model group: " << move_group_name_); - } - - x_pid_config_.dt = servo_parameters_.publish_period; - y_pid_config_.dt = servo_parameters_.publish_period; - z_pid_config_.dt = servo_parameters_.publish_period; - angular_pid_config_.dt = servo_parameters_.publish_period; - - x_pid_config_.windup_limit = servo_parameters_.windup_limit; - y_pid_config_.windup_limit = servo_parameters_.windup_limit; - z_pid_config_.windup_limit = servo_parameters_.windup_limit; - angular_pid_config_.windup_limit = servo_parameters_.windup_limit; - - x_pid_config_.k_p = servo_parameters_.x_proportional_gain; - x_pid_config_.k_i = servo_parameters_.x_integral_gain; - x_pid_config_.k_d = servo_parameters_.x_derivative_gain; - - y_pid_config_.k_p = servo_parameters_.y_proportional_gain; - y_pid_config_.k_i = servo_parameters_.y_integral_gain; - y_pid_config_.k_d = servo_parameters_.y_derivative_gain; - - z_pid_config_.k_p = servo_parameters_.z_proportional_gain; - z_pid_config_.k_i = servo_parameters_.z_integral_gain; - z_pid_config_.k_d = servo_parameters_.z_derivative_gain; - - angular_pid_config_.k_p = servo_parameters_.angular_proportional_gain; - angular_pid_config_.k_i = servo_parameters_.angular_integral_gain; - angular_pid_config_.k_d = servo_parameters_.angular_derivative_gain; -} - -void PoseTracking::initializePID(const PIDConfig& pid_config, std::vector& pid_vector) -{ - bool use_anti_windup = true; - pid_vector.push_back(control_toolbox::Pid(pid_config.k_p, pid_config.k_i, pid_config.k_d, pid_config.windup_limit, - -pid_config.windup_limit, use_anti_windup)); -} - -bool PoseTracking::haveRecentTargetPose(const double timespan) -{ - std::lock_guard lock(target_pose_mtx_); - return ((node_->now() - target_pose_.header.stamp).seconds() < timespan); -} - -bool PoseTracking::haveRecentEndEffectorPose(const double timespan) -{ - return ((node_->now() - command_frame_transform_stamp_).seconds() < timespan); -} - -bool PoseTracking::satisfiesPoseTolerance(const Eigen::Vector3d& positional_tolerance, const double angular_tolerance) -{ - std::lock_guard lock(target_pose_mtx_); - double x_error = target_pose_.pose.position.x - command_frame_transform_.translation()(0); - double y_error = target_pose_.pose.position.y - command_frame_transform_.translation()(1); - double z_error = target_pose_.pose.position.z - command_frame_transform_.translation()(2); - - // If uninitialized, likely haven't received the target pose yet. - if (!angular_error_) - return false; - - return ((std::abs(x_error) < positional_tolerance(0)) && (std::abs(y_error) < positional_tolerance(1)) && - (std::abs(z_error) < positional_tolerance(2)) && (std::abs(*angular_error_) < angular_tolerance)); -} - -void PoseTracking::targetPoseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg) -{ - std::lock_guard lock(target_pose_mtx_); - target_pose_ = *msg; - // If the target pose is not defined in planning frame, transform the target pose. - if (target_pose_.header.frame_id != planning_frame_) - { - try - { - geometry_msgs::msg::TransformStamped target_to_planning_frame = transform_buffer_.lookupTransform( - planning_frame_, target_pose_.header.frame_id, rclcpp::Time(0), rclcpp::Duration(100ms)); - tf2::doTransform(target_pose_, target_pose_, target_to_planning_frame); - - // Prevent doTransform from copying a stamp of 0, which will cause the haveRecentTargetPose check to fail servo motions - target_pose_.header.stamp = node_->now(); - } - catch (const tf2::TransformException& ex) - { - RCLCPP_WARN_STREAM(LOGGER, ex.what()); - return; - } - } -} - -geometry_msgs::msg::TwistStamped::ConstSharedPtr PoseTracking::calculateTwistCommand() -{ - // use the shared pool to create a message more efficiently - auto msg = moveit::util::make_shared_from_pool(); - - // Get twist components from PID controllers - geometry_msgs::msg::Twist& twist = msg->twist; - Eigen::Quaterniond q_desired; - - // Scope mutex locking only to operations which require access to target pose. - { - std::lock_guard lock(target_pose_mtx_); - msg->header.frame_id = target_pose_.header.frame_id; - - // Position - twist.linear.x = cartesian_position_pids_[0].computeCommand( - target_pose_.pose.position.x - command_frame_transform_.translation()(0), loop_rate_.period().count()); - twist.linear.y = cartesian_position_pids_[1].computeCommand( - target_pose_.pose.position.y - command_frame_transform_.translation()(1), loop_rate_.period().count()); - twist.linear.z = cartesian_position_pids_[2].computeCommand( - target_pose_.pose.position.z - command_frame_transform_.translation()(2), loop_rate_.period().count()); - - // Orientation algorithm: - // - Find the orientation error as a quaternion: q_error = q_desired * q_current ^ -1 - // - Use the angle-axis PID controller to calculate an angular rate - // - Convert to angular velocity for the TwistStamped message - q_desired = Eigen::Quaterniond(target_pose_.pose.orientation.w, target_pose_.pose.orientation.x, - target_pose_.pose.orientation.y, target_pose_.pose.orientation.z); - } - - Eigen::Quaterniond q_current(command_frame_transform_.rotation()); - Eigen::Quaterniond q_error = q_desired * q_current.inverse(); - - // Convert axis-angle to angular velocity - Eigen::AngleAxisd axis_angle(q_error); - // Cache the angular error, for rotation tolerance checking - angular_error_ = axis_angle.angle(); - - double ang_vel_magnitude = - cartesian_orientation_pids_[0].computeCommand(*angular_error_, loop_rate_.period().count()); - twist.angular.x = ang_vel_magnitude * axis_angle.axis()[0]; - twist.angular.y = ang_vel_magnitude * axis_angle.axis()[1]; - twist.angular.z = ang_vel_magnitude * axis_angle.axis()[2]; - - msg->header.stamp = node_->now(); - - return msg; -} - -void PoseTracking::stopMotion() -{ - stop_requested_ = true; - - // Send a 0 command to Servo to halt arm motion - auto msg = moveit::util::make_shared_from_pool(); - { - std::lock_guard lock(target_pose_mtx_); - msg->header.frame_id = target_pose_.header.frame_id; - } - msg->header.stamp = node_->now(); - twist_stamped_pub_->publish(*msg); -} - -void PoseTracking::doPostMotionReset() -{ - stopMotion(); - stop_requested_ = false; - angular_error_ = {}; - - // Reset error integrals and previous errors of PID controllers - cartesian_position_pids_[0].reset(); - cartesian_position_pids_[1].reset(); - cartesian_position_pids_[2].reset(); - cartesian_orientation_pids_[0].reset(); -} - -void PoseTracking::updatePIDConfig(const double x_proportional_gain, const double x_integral_gain, - const double x_derivative_gain, const double y_proportional_gain, - const double y_integral_gain, const double y_derivative_gain, - const double z_proportional_gain, const double z_integral_gain, - const double z_derivative_gain, const double angular_proportional_gain, - const double angular_integral_gain, const double angular_derivative_gain) -{ - stopMotion(); - - x_pid_config_.k_p = x_proportional_gain; - x_pid_config_.k_i = x_integral_gain; - x_pid_config_.k_d = x_derivative_gain; - y_pid_config_.k_p = y_proportional_gain; - y_pid_config_.k_i = y_integral_gain; - y_pid_config_.k_d = y_derivative_gain; - z_pid_config_.k_p = z_proportional_gain; - z_pid_config_.k_i = z_integral_gain; - z_pid_config_.k_d = z_derivative_gain; - - angular_pid_config_.k_p = angular_proportional_gain; - angular_pid_config_.k_i = angular_integral_gain; - angular_pid_config_.k_d = angular_derivative_gain; - - cartesian_position_pids_.clear(); - cartesian_orientation_pids_.clear(); - initializePID(x_pid_config_, cartesian_position_pids_); - initializePID(y_pid_config_, cartesian_position_pids_); - initializePID(z_pid_config_, cartesian_position_pids_); - initializePID(angular_pid_config_, cartesian_orientation_pids_); - - doPostMotionReset(); -} - -void PoseTracking::getPIDErrors(double& x_error, double& y_error, double& z_error, double& orientation_error) -{ - double dummy1, dummy2; - cartesian_position_pids_.at(0).getCurrentPIDErrors(x_error, dummy1, dummy2); - cartesian_position_pids_.at(1).getCurrentPIDErrors(y_error, dummy1, dummy2); - cartesian_position_pids_.at(2).getCurrentPIDErrors(z_error, dummy1, dummy2); - cartesian_orientation_pids_.at(0).getCurrentPIDErrors(orientation_error, dummy1, dummy2); -} - -void PoseTracking::resetTargetPose() -{ - std::lock_guard lock(target_pose_mtx_); - target_pose_ = geometry_msgs::msg::PoseStamped(); - target_pose_.header.stamp = rclcpp::Time(RCL_ROS_TIME); -} - -bool PoseTracking::getCommandFrameTransform(geometry_msgs::msg::TransformStamped& transform) -{ - return servo_->getCommandFrameTransform(transform); -} -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 7bbe6c55c3..91763f9afe 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -33,74 +33,491 @@ /* Title : servo.cpp * Project : moveit_servo - * Created : 3/9/2017 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + * Created : 05/17/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim */ -#include -#include +#include +#include +#include +#include -namespace moveit_servo -{ namespace { const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo"); -constexpr double ROBOT_STATE_WAIT_TIME = 10.0; // seconds +constexpr double ROBOT_STATE_WAIT_TIME = 5.0; // seconds +constexpr double STOPPED_VELOCITY_EPS = 1e-4; } // namespace -Servo::Servo(const rclcpp::Node::SharedPtr& node, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& servo_param_listener) - : servo_param_listener_(servo_param_listener) - , servo_params_{ servo_param_listener_->get_params() } - , planning_scene_monitor_{ planning_scene_monitor } - , servo_calcs_{ node, planning_scene_monitor_, servo_param_listener_ } - , collision_checker_{ node, planning_scene_monitor_, servo_param_listener_ } +namespace moveit_servo { -} -void Servo::start() +Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr servo_param_listener, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) + : node_(node) + , servo_param_listener_{ std::move(servo_param_listener) } + , planning_scene_monitor_{ planning_scene_monitor } + , transform_buffer_(node_->get_clock()) + , transform_listener_(transform_buffer_) { + servo_params_ = servo_param_listener_->get_params(); + + const bool params_valid = validateParams(servo_params_); + if (!params_valid) + { + RCLCPP_ERROR_STREAM(LOGGER, "Got invalid parameters, exiting."); + std::exit(EXIT_FAILURE); + } + if (!planning_scene_monitor_->getStateMonitor()->waitForCompleteState(servo_params_.move_group_name, ROBOT_STATE_WAIT_TIME)) { RCLCPP_ERROR(LOGGER, "Timeout waiting for current state"); - return; + std::exit(EXIT_FAILURE); } - servo_params_ = servo_param_listener_->get_params(); + // Planning scene monitor is passed in. + if (servo_params_.is_primary_planning_scene_monitor) + { + planning_scene_monitor_->providePlanningSceneService(); + } + else + { + planning_scene_monitor_->requestPlanningSceneState(); + } + + moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + // Check if the transforms to planning frame and end effector frame exist. + if (!robot_state->knowsFrameTransform(servo_params_.planning_frame)) + { + servo_status_ = StatusCode::INVALID; + RCLCPP_ERROR_STREAM(LOGGER, "No transform available for planning frame " << servo_params_.planning_frame); + } + else if (!robot_state->knowsFrameTransform(servo_params_.ee_frame)) + { + servo_status_ = StatusCode::INVALID; + RCLCPP_ERROR_STREAM(LOGGER, "No transform available for end effector frame " << servo_params_.ee_frame); + } + else + { + // Load the smoothing plugin + if (servo_params_.use_smoothing) + { + setSmoothingPlugin(); + } + else + { + RCLCPP_WARN(LOGGER, "No smoothing plugin loaded"); + } + + // Create the collision checker and start collision checking. + collision_monitor_ = + std::make_unique(planning_scene_monitor_, servo_params_, std::ref(collision_velocity_scale_)); + collision_monitor_->start(); + + servo_status_ = StatusCode::NO_WARNING; + RCLCPP_INFO_STREAM(LOGGER, "Servo initialized successfully"); + } +} + +Servo::~Servo() +{ + setCollisionChecking(false); +} + +void Servo::setSmoothingPlugin() +{ + // Load the smoothing plugin + try + { + pluginlib::ClassLoader smoothing_loader( + "moveit_core", "online_signal_smoothing::SmoothingBaseClass"); + smoother_ = smoothing_loader.createUniqueInstance(servo_params_.smoothing_filter_plugin_name); + } + catch (pluginlib::PluginlibException& ex) + { + RCLCPP_ERROR(LOGGER, "Exception while loading the smoothing plugin '%s': '%s'", + servo_params_.smoothing_filter_plugin_name.c_str(), ex.what()); + std::exit(EXIT_FAILURE); + } + + // Initialize the smoothing plugin + moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + const int num_joints = + robot_state->getJointModelGroup(servo_params_.move_group_name)->getActiveJointModelNames().size(); + if (!smoother_->initialize(node_, planning_scene_monitor_->getRobotModel(), num_joints)) + { + RCLCPP_ERROR(LOGGER, "Smoothing plugin could not be initialized"); + std::exit(EXIT_FAILURE); + } +} + +void Servo::setCollisionChecking(const bool check_collision) +{ + check_collision ? collision_monitor_->start() : collision_monitor_->stop(); +} + +bool Servo::validateParams(const servo::Params& servo_params) +{ + bool params_valid = true; + + auto robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + auto joint_model_group = robot_state->getJointModelGroup(servo_params.move_group_name); + if (joint_model_group == nullptr) + { + RCLCPP_ERROR_STREAM(LOGGER, "Invalid move group name: `" << servo_params.move_group_name << '`'); + params_valid = false; + } + + if (servo_params.hard_stop_singularity_threshold <= servo_params.lower_singularity_threshold) + { + RCLCPP_ERROR(LOGGER, "Parameter 'hard_stop_singularity_threshold' " + "should be greater than 'lower_singularity_threshold.' " + "Check the parameters YAML file used to launch this node."); + params_valid = false; + } + + if (!servo_params.publish_joint_positions && !servo_params.publish_joint_velocities && + !servo_params.publish_joint_accelerations) + { + RCLCPP_ERROR(LOGGER, "At least one of publish_joint_positions / " + "publish_joint_velocities / " + "publish_joint_accelerations must be true. " + "Check the parameters YAML file used to launch this node."); + params_valid = false; + } + + if ((servo_params.command_out_type == "std_msgs/Float64MultiArray") && servo_params.publish_joint_positions && + servo_params.publish_joint_velocities) + { + RCLCPP_ERROR(LOGGER, "When publishing a std_msgs/Float64MultiArray, " + "you must select positions OR velocities." + "Check the parameters YAML file used to launch this node."); + params_valid = false; + } + + if (servo_params.scene_collision_proximity_threshold < servo_params.self_collision_proximity_threshold) + { + RCLCPP_ERROR(LOGGER, "Parameter 'self_collision_proximity_threshold' should probably be less " + "than or equal to 'scene_collision_proximity_threshold'." + "Check the parameters YAML file used to launch this node."); + params_valid = false; + } + + return params_valid; +} + +bool Servo::updateParams() +{ + bool params_updated = false; - // Crunch the numbers in this timer - servo_calcs_.start(); + if (servo_param_listener_->is_old(servo_params_)) + { + auto params = servo_param_listener_->get_params(); + + const bool params_valid = validateParams(params); + if (params_valid) + { + if (params.override_velocity_scaling_factor != servo_params_.override_velocity_scaling_factor) + { + RCLCPP_INFO_STREAM(LOGGER, "override_velocity_scaling_factor changed to : " + << std::to_string(params.override_velocity_scaling_factor)); + } + if (params.move_group_name != servo_params_.move_group_name) + { + RCLCPP_INFO_STREAM(LOGGER, "Move group changed from " << servo_params_.move_group_name << " to " + << params.move_group_name); + } - // Check collisions in this timer - if (servo_params_.check_collisions) - collision_checker_.start(); + servo_params_ = params; + params_updated = true; + } + else + { + RCLCPP_WARN_STREAM(LOGGER, "Parameters will not be updated."); + } + } + return params_updated; } -void Servo::stop() +servo::Params& Servo::getParams() { - servo_calcs_.stop(); - collision_checker_.stop(); + return servo_params_; } -bool Servo::getCommandFrameTransform(Eigen::Isometry3d& transform) +StatusCode Servo::getStatus() { - return servo_calcs_.getCommandFrameTransform(transform); + return servo_status_; } -bool Servo::getCommandFrameTransform(geometry_msgs::msg::TransformStamped& transform) +const std::string Servo::getStatusMessage() { - return servo_calcs_.getCommandFrameTransform(transform); + return SERVO_STATUS_CODE_MAP.at(servo_status_); } -bool Servo::getEEFrameTransform(Eigen::Isometry3d& transform) +CommandType Servo::getCommandType() { - return servo_calcs_.getEEFrameTransform(transform); + return expected_command_type_; } -bool Servo::getEEFrameTransform(geometry_msgs::msg::TransformStamped& transform) +void Servo::setCommandType(const CommandType& command_type) { - return servo_calcs_.getEEFrameTransform(transform); + expected_command_type_ = command_type; } + +const Eigen::Isometry3d Servo::getEndEffectorPose() +{ + // Robot base (panda_link0) to end effector frame (panda_link8) + return planning_scene_monitor_->getStateMonitor()->getCurrentState()->getGlobalLinkTransform(servo_params_.ee_frame); +} + +KinematicState Servo::haltJoints(const std::vector& joints_to_halt, const KinematicState& current_state, + const KinematicState& target_state) +{ + KinematicState bounded_state(target_state.joint_names.size()); + bounded_state.joint_names = target_state.joint_names; + + std::stringstream halting_joint_names; + for (const int idx : joints_to_halt) + { + halting_joint_names << bounded_state.joint_names[idx] + " "; + } + RCLCPP_WARN_STREAM(LOGGER, "Joint position limit reached on joints: " << halting_joint_names.str()); + + const bool all_joint_halt = + (getCommandType() == CommandType::JOINT_JOG && servo_params_.halt_all_joints_in_joint_mode) || + (getCommandType() == CommandType::TWIST && servo_params_.halt_all_joints_in_cartesian_mode); + + if (all_joint_halt) + { + // The velocities are initialized to zero by default, so we dont need to set it here. + bounded_state.positions = current_state.positions; + } + else + { + // Halt only the joints that are out of bounds + bounded_state.positions = target_state.positions; + bounded_state.velocities = target_state.velocities; + for (const int idx : joints_to_halt) + { + bounded_state.positions[idx] = current_state.positions[idx]; + bounded_state.velocities[idx] = 0.0; + } + } + + return bounded_state; +} + +Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command, moveit::core::RobotStatePtr& robot_state) +{ + const int num_joints = + robot_state->getJointModelGroup(servo_params_.move_group_name)->getActiveJointModelNames().size(); + Eigen::VectorXd joint_position_deltas(num_joints); + joint_position_deltas.setZero(); + + JointDeltaResult delta_result; + + const CommandType expected_type = getCommandType(); + if (command.index() == static_cast(expected_type)) + { + if (expected_type == CommandType::JOINT_JOG) + { + delta_result = jointDeltaFromJointJog(std::get(command), robot_state, servo_params_); + servo_status_ = delta_result.first; + } + else if (expected_type == CommandType::TWIST) + { + try + { + const TwistCommand command_in_planning_frame = toPlanningFrame(std::get(command)); + delta_result = jointDeltaFromTwist(command_in_planning_frame, robot_state, servo_params_); + servo_status_ = delta_result.first; + } + catch (tf2::TransformException& ex) + { + servo_status_ = StatusCode::INVALID; + RCLCPP_ERROR_STREAM(LOGGER, "Could not transform twist to planning frame."); + } + } + else if (expected_type == CommandType::POSE) + { + try + { + const PoseCommand command_in_planning_frame = toPlanningFrame(std::get(command)); + delta_result = jointDeltaFromPose(command_in_planning_frame, robot_state, servo_params_); + servo_status_ = delta_result.first; + } + catch (tf2::TransformException& ex) + { + servo_status_ = StatusCode::INVALID; + RCLCPP_ERROR_STREAM(LOGGER, "Could not transform pose to planning frame."); + } + } + + if (servo_status_ != StatusCode::INVALID) + { + joint_position_deltas = delta_result.second; + } + } + else + { + servo_status_ = StatusCode::INVALID; + RCLCPP_WARN_STREAM(LOGGER, "SERVO : Incoming command type does not match expected command type."); + } + + return joint_position_deltas; +} + +KinematicState Servo::getNextJointState(const ServoInput& command) +{ + // Set status to clear + servo_status_ = StatusCode::NO_WARNING; + + // Update the parameters + updateParams(); + + // Get the robot state and joint model group info. + moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + const moveit::core::JointModelGroup* joint_model_group = + robot_state->getJointModelGroup(servo_params_.move_group_name); + + // Get necessary information about joints + const std::vector joint_names = joint_model_group->getActiveJointModelNames(); + const moveit::core::JointBoundsVector joint_bounds = joint_model_group->getActiveJointModelsBounds(); + const int num_joints = joint_names.size(); + + // State variables + KinematicState current_state(num_joints), target_state(num_joints); + target_state.joint_names = joint_names; + + // Copy current kinematic data from RobotState. + robot_state->copyJointGroupPositions(joint_model_group, current_state.positions); + robot_state->copyJointGroupVelocities(joint_model_group, current_state.velocities); + + // Create Eigen maps for cleaner operations. + Eigen::Map current_joint_positions(current_state.positions.data(), num_joints); + Eigen::Map target_joint_positions(target_state.positions.data(), num_joints); + Eigen::Map current_joint_velocities(current_state.velocities.data(), num_joints); + Eigen::Map target_joint_velocities(target_state.velocities.data(), num_joints); + + // Compute the change in joint position due to the incoming command + Eigen::VectorXd joint_position_delta = jointDeltaFromCommand(command, robot_state); + + if (collision_velocity_scale_ > 0 && collision_velocity_scale_ < 1) + { + servo_status_ = StatusCode::DECELERATE_FOR_COLLISION; + } + else if (collision_velocity_scale_ == 0) + { + servo_status_ = StatusCode::HALT_FOR_COLLISION; + } + + // Continue rest of the computations only if the command is valid + // The computations can be skipped also in case we are halting. + if (servo_status_ != StatusCode::INVALID && servo_status_ != StatusCode::HALT_FOR_COLLISION) + { + // Apply collision scaling to the joint position delta + joint_position_delta *= collision_velocity_scale_; + + // Compute the next joint positions based on the joint position deltas + target_joint_positions = current_joint_positions + joint_position_delta; + + // TODO : apply filtering to the velocity instead of position + // Apply smoothing to the positions if a smoother was provided. + // Update filter state and apply filtering in position domain + if (smoother_) + { + smoother_->reset(current_state.positions); + smoother_->doSmoothing(target_state.positions); + } + + // Compute velocities based on smoothed joint positions + target_joint_velocities = (target_joint_positions - current_joint_positions) / servo_params_.publish_period; + + // Scale down the velocity based on joint velocity limit or user defined scaling if applicable. + const double joint_limit_scale = jointLimitVelocityScalingFactor(target_joint_velocities, joint_bounds, + servo_params_.override_velocity_scaling_factor); + if (joint_limit_scale < 1.0) // 1.0 means no scaling. + RCLCPP_WARN_STREAM(LOGGER, "Joint velocity limit scaling applied by a factor of " << joint_limit_scale); + + target_joint_velocities *= joint_limit_scale; + + // Adjust joint position based on scaled down velocity + target_joint_positions = current_joint_positions + (target_joint_velocities * servo_params_.publish_period); + + // Check if any joints are going past joint position limits + const std::vector joints_to_halt = + jointsToHalt(target_joint_positions, target_joint_velocities, joint_bounds, servo_params_.joint_limit_margin); + + // Apply halting if any joints need to be halted. + if (!joints_to_halt.empty()) + { + servo_status_ = StatusCode::JOINT_BOUND; + target_state = haltJoints(joints_to_halt, current_state, target_state); + } + } + + return target_state; +} + +const TwistCommand Servo::toPlanningFrame(const TwistCommand& command) +{ + const auto command_to_planning_frame = + transform_buffer_.lookupTransform(servo_params_.planning_frame, command.frame_id, rclcpp::Time(0)); + Eigen::VectorXd transformed_twist = command.velocities; + tf2::doTransform(transformed_twist, transformed_twist, command_to_planning_frame); + return TwistCommand{ servo_params_.planning_frame, transformed_twist }; +} + +const PoseCommand Servo::toPlanningFrame(const PoseCommand& command) +{ + auto target_pose_msg = convertIsometryToTransform(command.pose, servo_params_.planning_frame, command.frame_id); + auto command_to_planning_frame = transform_buffer_.lookupTransform( + servo_params_.planning_frame, command.frame_id, rclcpp::Time(0), rclcpp::Duration::from_seconds(2)); + tf2::doTransform(target_pose_msg, target_pose_msg, command_to_planning_frame); + return PoseCommand{ servo_params_.planning_frame, tf2::transformToEigen(target_pose_msg) }; +} + +KinematicState Servo::getCurrentRobotState() +{ + moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + const moveit::core::JointModelGroup* joint_model_group = + robot_state->getJointModelGroup(servo_params_.move_group_name); + const auto joint_names = joint_model_group->getActiveJointModelNames(); + + KinematicState current_state(joint_names.size()); + current_state.joint_names = joint_names; + robot_state->copyJointGroupPositions(joint_model_group, current_state.positions); + robot_state->copyJointGroupVelocities(joint_model_group, current_state.velocities); + robot_state->copyJointGroupAccelerations(joint_model_group, current_state.accelerations); + + return current_state; +} + +std::pair Servo::smoothHalt(KinematicState halt_state) +{ + bool stopped = false; + const auto current_state = getCurrentRobotState(); + + const size_t num_joints = current_state.joint_names.size(); + for (size_t i = 0; i < num_joints; i++) + { + const double vel = (halt_state.positions[i] - current_state.positions[i]) / servo_params_.publish_period; + halt_state.velocities[i] = (vel > STOPPED_VELOCITY_EPS) ? vel : 0.0; + halt_state.accelerations[i] = + (halt_state.velocities[i] - current_state.velocities[i]) / servo_params_.publish_period; + } + + // If all velocities are zero, robot has decelerated to a stop. + stopped = (std::accumulate(halt_state.velocities.begin(), halt_state.velocities.end(), 0.0) == 0.0); + + if (smoother_) + { + smoother_->reset(current_state.positions); + smoother_->doSmoothing(halt_state.positions); + } + + return std::make_pair(stopped, halt_state); +} + } // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp deleted file mode 100644 index ef185c25d1..0000000000 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ /dev/null @@ -1,966 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * 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 copyright holder 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 HOLDER 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. - *******************************************************************************/ - -/* Title : servo_calcs.cpp - * Project : moveit_servo - * Created : 1/11/2019 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - */ - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -// Disable -Wold-style-cast because all _THROTTLE macros trigger this -// It would be too noisy to disable on a per-callsite basis -#pragma GCC diagnostic ignored "-Wold-style-cast" - -using namespace std::chrono_literals; // for s, ms, etc. - -namespace moveit_servo -{ -namespace -{ -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_calcs"); -constexpr auto ROS_LOG_THROTTLE_PERIOD = std::chrono::milliseconds(3000).count(); -static constexpr double STOPPED_VELOCITY_EPS = 1e-4; // rad/s - -// This value is used when configuring the main loop to use SCHED_FIFO scheduling -// We use a slightly lower priority than the ros2_control default in order to reduce jitter -// Reference: https://man7.org/linux/man-pages/man2/sched_setparam.2.html -int const THREAD_PRIORITY = 40; -} // namespace - -// Constructor for the class that handles servoing calculations -ServoCalcs::ServoCalcs(const rclcpp::Node::SharedPtr& node, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& servo_param_listener) - : node_(node) - , servo_param_listener_(servo_param_listener) - , servo_params_(servo_param_listener_->get_params()) - , planning_scene_monitor_(planning_scene_monitor) - , stop_requested_(true) - , smoothing_loader_("moveit_core", "online_signal_smoothing::SmoothingBaseClass") - -{ - // MoveIt Setup - current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); - joint_model_group_ = current_state_->getJointModelGroup(servo_params_.move_group_name); - if (joint_model_group_ == nullptr) - { - RCLCPP_ERROR_STREAM(LOGGER, "Invalid move group name: `" << servo_params_.move_group_name << '`'); - throw std::runtime_error("Invalid move group name"); - } - - // Subscribe to command topics - twist_stamped_sub_ = node_->create_subscription( - servo_params_.cartesian_command_in_topic, rclcpp::SystemDefaultsQoS(), - [this](const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg) { return twistStampedCB(msg); }); - - joint_cmd_sub_ = node_->create_subscription( - servo_params_.joint_command_in_topic, rclcpp::SystemDefaultsQoS(), - [this](const control_msgs::msg::JointJog::ConstSharedPtr& msg) { return jointCmdCB(msg); }); - - // Subscribe to the collision_check topic - collision_velocity_scale_sub_ = node_->create_subscription( - "~/collision_velocity_scale", rclcpp::SystemDefaultsQoS(), - [this](const std_msgs::msg::Float64::ConstSharedPtr& msg) { return collisionVelocityScaleCB(msg); }); - - // Publish freshly-calculated joints to the robot. - // Put the outgoing msg in the right format (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - if (servo_params_.command_out_type == "trajectory_msgs/JointTrajectory") - { - trajectory_outgoing_cmd_pub_ = node_->create_publisher( - servo_params_.command_out_topic, rclcpp::SystemDefaultsQoS()); - } - else if (servo_params_.command_out_type == "std_msgs/Float64MultiArray") - { - multiarray_outgoing_cmd_pub_ = node_->create_publisher( - servo_params_.command_out_topic, rclcpp::SystemDefaultsQoS()); - } - - // Publish status - status_pub_ = node_->create_publisher(servo_params_.status_topic, rclcpp::SystemDefaultsQoS()); - - internal_joint_state_.name = joint_model_group_->getActiveJointModelNames(); - num_joints_ = internal_joint_state_.name.size(); - internal_joint_state_.position.resize(num_joints_); - internal_joint_state_.velocity.resize(num_joints_); - delta_theta_.setZero(num_joints_); - - for (std::size_t i = 0; i < num_joints_; ++i) - { - // A map for the indices of incoming joint commands - joint_state_name_map_[internal_joint_state_.name[i]] = i; - } - - // Load the smoothing plugin - try - { - smoother_ = smoothing_loader_.createUniqueInstance(servo_params_.smoothing_filter_plugin_name); - } - catch (pluginlib::PluginlibException& ex) - { - RCLCPP_ERROR(LOGGER, "Exception while loading the smoothing plugin '%s': '%s'", - servo_params_.smoothing_filter_plugin_name.c_str(), ex.what()); - std::exit(EXIT_FAILURE); - } - - // Initialize the smoothing plugin - if (!smoother_->initialize(node_, planning_scene_monitor_->getRobotModel(), num_joints_)) - { - RCLCPP_ERROR(LOGGER, "Smoothing plugin could not be initialized"); - std::exit(EXIT_FAILURE); - } - - // A matrix of all zeros is used to check whether matrices have been initialized - Eigen::Matrix3d empty_matrix; - empty_matrix.setZero(); - tf_moveit_to_ee_frame_ = empty_matrix; - tf_moveit_to_robot_cmd_frame_ = empty_matrix; - - // Get the IK solver for the group - ik_solver_ = joint_model_group_->getSolverInstance(); - if (!ik_solver_) - { - RCLCPP_WARN( - LOGGER, - "No kinematics solver instantiated for group '%s'. Will use inverse Jacobian for servo calculations instead.", - joint_model_group_->getName().c_str()); - } - else if (!ik_solver_->supportsGroup(joint_model_group_)) - { - ik_solver_ = nullptr; - RCLCPP_WARN(LOGGER, - "The loaded kinematics plugin does not support group '%s'. Will use inverse Jacobian for servo " - "calculations instead.", - joint_model_group_->getName().c_str()); - } -} - -ServoCalcs::~ServoCalcs() -{ - stop(); -} - -void ServoCalcs::start() -{ - // Stop the thread if we are currently running - stop(); - - // Set up the "last" published message, in case we need to send it first - auto initial_joint_trajectory = std::make_unique(); - initial_joint_trajectory->header.stamp = node_->now(); - initial_joint_trajectory->header.frame_id = servo_params_.planning_frame; - initial_joint_trajectory->joint_names = internal_joint_state_.name; - trajectory_msgs::msg::JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(servo_params_.publish_period); - if (servo_params_.publish_joint_positions) - { - planning_scene_monitor_->getStateMonitor()->getCurrentState()->copyJointGroupPositions(joint_model_group_, - point.positions); - } - if (servo_params_.publish_joint_velocities) - { - std::vector velocity(num_joints_); - point.velocities = velocity; - } - if (servo_params_.publish_joint_accelerations) - { - // I do not know of a robot that takes acceleration commands. - // However, some controllers check that this data is non-empty. - // Send all zeros, for now. - point.accelerations.resize(num_joints_); - } - initial_joint_trajectory->points.push_back(point); - last_sent_command_ = std::move(initial_joint_trajectory); - - current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); - current_state_->copyJointGroupPositions(joint_model_group_, internal_joint_state_.position); - current_state_->copyJointGroupVelocities(joint_model_group_, internal_joint_state_.velocity); - // set previous state to same as internal state for t = 0 - previous_joint_state_ = internal_joint_state_; - - // Check that all links are known to the robot - auto check_link_is_known = [this](const std::string& frame_name) { - if (!current_state_->knowsFrameTransform(frame_name)) - { - throw std::runtime_error{ "Unknown frame: " + frame_name }; - } - }; - check_link_is_known(servo_params_.planning_frame); - check_link_is_known(servo_params_.ee_frame_name); - check_link_is_known(servo_params_.robot_link_command_frame); - - tf_moveit_to_ee_frame_ = current_state_->getGlobalLinkTransform(servo_params_.planning_frame).inverse() * - current_state_->getGlobalLinkTransform(servo_params_.ee_frame_name); - tf_moveit_to_robot_cmd_frame_ = current_state_->getGlobalLinkTransform(servo_params_.planning_frame).inverse() * - current_state_->getGlobalLinkTransform(servo_params_.robot_link_command_frame); - - // Always reset the low-pass filters when first starting servo - resetLowPassFilters(internal_joint_state_); - - stop_requested_ = false; - thread_ = std::thread([this] { - // Check if a realtime kernel is installed. Set a higher thread priority, if so - if (realtime_tools::has_realtime_kernel()) - { - if (!realtime_tools::configure_sched_fifo(THREAD_PRIORITY)) - { - RCLCPP_WARN(LOGGER, "Could not enable FIFO RT scheduling policy"); - } - } - else - { - RCLCPP_INFO(LOGGER, "RT kernel is recommended for better performance"); - } - mainCalcLoop(); - }); - new_input_cmd_ = false; -} - -void ServoCalcs::stop() -{ - // Request stop - stop_requested_ = true; - - // Notify condition variable in case the thread is blocked on it - { - // scope so the mutex is unlocked after so the thread can continue - // and therefore be joinable - const std::lock_guard lock(main_loop_mutex_); - new_input_cmd_ = false; - input_cv_.notify_all(); - } - - // Join the thread - if (thread_.joinable()) - { - thread_.join(); - } -} - -void ServoCalcs::updateParams() -{ - if (servo_param_listener_->is_old(servo_params_)) - { - auto params = servo_param_listener_->get_params(); - if (params.override_velocity_scaling_factor != servo_params_.override_velocity_scaling_factor) - { - RCLCPP_INFO_STREAM(LOGGER, "override_velocity_scaling_factor changed to : " - << std::to_string(params.override_velocity_scaling_factor)); - } - - if (params.robot_link_command_frame != servo_params_.robot_link_command_frame) - { - if (current_state_->knowsFrameTransform(params.robot_link_command_frame)) - { - RCLCPP_INFO_STREAM(LOGGER, "robot_link_command_frame changed to : " << params.robot_link_command_frame); - } - else - { - RCLCPP_ERROR_STREAM(LOGGER, "Failed to change robot_link_command_frame. Passed frame '" - << params.robot_link_command_frame - << "' is unknown, will keep using old command frame."); - // Replace frame in new param set with old frame value - // TODO : Is there a better behaviour here ? - params.robot_link_command_frame = servo_params_.robot_link_command_frame; - } - } - servo_params_ = params; - } -} - -void ServoCalcs::mainCalcLoop() -{ - rclcpp::WallRate rate(1.0 / servo_params_.publish_period); - - while (rclcpp::ok() && !stop_requested_) - { - // lock the input state mutex - std::unique_lock main_loop_lock(main_loop_mutex_); - - // Check if any parameters changed - if (servo_params_.enable_parameter_update) - { - updateParams(); - } - - // low latency mode -- begin calculations as soon as a new command is received. - if (servo_params_.low_latency_mode) - { - input_cv_.wait(main_loop_lock, [this] { return (new_input_cmd_ || stop_requested_); }); - } - - // reset new_input_cmd_ flag - new_input_cmd_ = false; - - // run servo calcs - const auto start_time = node_->now(); - calculateSingleIteration(); - const auto run_duration = node_->now() - start_time; - - // Log warning when the run duration was longer than the period - if (run_duration.seconds() > servo_params_.publish_period) - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "run_duration: " << run_duration.seconds() << " (" << servo_params_.publish_period - << ')'); - } - - // normal mode, unlock input mutex and wait for the period of the loop - if (!servo_params_.low_latency_mode) - { - main_loop_lock.unlock(); - rate.sleep(); - } - } -} - -void ServoCalcs::calculateSingleIteration() -{ - // Publish status each loop iteration - auto status_msg = std::make_unique(); - status_msg->data = static_cast(status_); - status_pub_->publish(std::move(status_msg)); - - // After we publish, status, reset it back to no warnings - status_ = StatusCode::NO_WARNING; - - // Always update the joints and end-effector transform for 2 reasons: - // 1) in case the getCommandFrameTransform() method is being used - // 2) so the low-pass filters are up to date and don't cause a jump - // Get the latest joint group positions - current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); - current_state_->copyJointGroupPositions(joint_model_group_, internal_joint_state_.position); - current_state_->copyJointGroupVelocities(joint_model_group_, internal_joint_state_.velocity); - - if (latest_twist_stamped_) - twist_stamped_cmd_ = *latest_twist_stamped_; - if (latest_joint_cmd_) - joint_servo_cmd_ = *latest_joint_cmd_; - - // Check for stale cmds - twist_command_is_stale_ = ((node_->now() - latest_twist_command_stamp_) >= - rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout)); - joint_command_is_stale_ = ((node_->now() - latest_joint_command_stamp_) >= - rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout)); - - // Get the transform from MoveIt planning frame to servoing command frame - // Calculate this transform to ensure it is available via C++ API - // We solve (planning_frame -> base -> robot_link_command_frame) - // by computing (base->planning_frame)^-1 * (base->robot_link_command_frame) - tf_moveit_to_robot_cmd_frame_ = current_state_->getGlobalLinkTransform(servo_params_.planning_frame).inverse() * - current_state_->getGlobalLinkTransform(servo_params_.robot_link_command_frame); - - // Calculate the transform from MoveIt planning frame to End Effector frame - // Calculate this transform to ensure it is available via C++ API - tf_moveit_to_ee_frame_ = current_state_->getGlobalLinkTransform(servo_params_.planning_frame).inverse() * - current_state_->getGlobalLinkTransform(servo_params_.ee_frame_name); - - // Don't end this function without updating the filters - updated_filters_ = false; - - // If waiting for initial servo commands, just keep the low-pass filters up to date with current - // joints so a jump doesn't occur when restarting - if (wait_for_servo_commands_) - { - resetLowPassFilters(internal_joint_state_); - - // Check if there are any new commands with valid timestamp - wait_for_servo_commands_ = - twist_stamped_cmd_.header.stamp == rclcpp::Time(0.) && joint_servo_cmd_.header.stamp == rclcpp::Time(0.); - - // Early exit - return; - } - - // If not waiting for initial command, - // Do servoing calculations only if the robot should move, for efficiency - // Create new outgoing joint trajectory command message - auto joint_trajectory = std::make_unique(); - - // Prioritize cartesian servoing above joint servoing - // Only run commands if not stale - if (!twist_command_is_stale_) - { - if (!cartesianServoCalcs(twist_stamped_cmd_, *joint_trajectory)) - { - resetLowPassFilters(internal_joint_state_); - return; - } - } - else if (!joint_command_is_stale_) - { - if (!jointServoCalcs(joint_servo_cmd_, *joint_trajectory)) - { - resetLowPassFilters(internal_joint_state_); - return; - } - } - - // Skip servoing publication if both types of commands are stale. - if (twist_command_is_stale_ && joint_command_is_stale_) - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_DEBUG_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "Skipping publishing because incoming commands are stale."); - filteredHalt(*joint_trajectory); - } - - // Clear out position commands if user did not request them (can cause interpolation issues) - if (!servo_params_.publish_joint_positions) - { - joint_trajectory->points[0].positions.clear(); - } - // Likewise for velocity and acceleration - if (!servo_params_.publish_joint_velocities) - { - joint_trajectory->points[0].velocities.clear(); - } - if (!servo_params_.publish_joint_accelerations) - { - joint_trajectory->points[0].accelerations.clear(); - } - - // Put the outgoing msg in the right format - // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - if (servo_params_.command_out_type == "trajectory_msgs/JointTrajectory") - { - // When a joint_trajectory_controller receives a new command, a stamp of 0 indicates "begin immediately" - // See http://wiki.ros.org/joint_trajectory_controller#Trajectory_replacement - joint_trajectory->header.stamp = rclcpp::Time(0); - *last_sent_command_ = *joint_trajectory; - trajectory_outgoing_cmd_pub_->publish(std::move(joint_trajectory)); - } - else if (servo_params_.command_out_type == "std_msgs/Float64MultiArray") - { - auto joints = std::make_unique(); - if (servo_params_.publish_joint_positions && !joint_trajectory->points.empty()) - { - joints->data = joint_trajectory->points[0].positions; - } - else if (servo_params_.publish_joint_velocities && !joint_trajectory->points.empty()) - { - joints->data = joint_trajectory->points[0].velocities; - } - *last_sent_command_ = *joint_trajectory; - multiarray_outgoing_cmd_pub_->publish(std::move(joints)); - } - - // Update the filters if we haven't yet - if (!updated_filters_) - resetLowPassFilters(internal_joint_state_); -} - -// Perform the servoing calculations -bool ServoCalcs::cartesianServoCalcs(geometry_msgs::msg::TwistStamped& cmd, - trajectory_msgs::msg::JointTrajectory& joint_trajectory) -{ - // Check for nan's in the incoming command - if (!checkValidCommand(cmd)) - return false; - - // Transform the command to the MoveGroup planning frame - if (cmd.header.frame_id.empty()) - { - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, - "No frame specified for command, will use planning_frame: " - << servo_params_.planning_frame); - cmd.header.frame_id = servo_params_.planning_frame; - } - if (cmd.header.frame_id != servo_params_.planning_frame) - { - transformTwistToPlanningFrame(cmd, servo_params_.planning_frame, current_state_); - } - - Eigen::VectorXd delta_x = scaleCartesianCommand(cmd); - - Eigen::MatrixXd jacobian = current_state_->getJacobian(joint_model_group_); - - Eigen::JacobiSVD svd = - Eigen::JacobiSVD(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); - Eigen::MatrixXd matrix_s = svd.singularValues().asDiagonal(); - Eigen::MatrixXd pseudo_inverse = svd.matrixV() * matrix_s.inverse() * svd.matrixU().transpose(); - - // Convert from cartesian commands to joint commands - // Use an IK solver plugin if we have one, otherwise use inverse Jacobian. - if (ik_solver_) - { - const Eigen::Isometry3d base_to_tip_frame_transform = - current_state_->getGlobalLinkTransform(ik_solver_->getBaseFrame()).inverse() * - current_state_->getGlobalLinkTransform(ik_solver_->getTipFrame()); - - geometry_msgs::msg::Pose next_pose = poseFromCartesianDelta(delta_x, base_to_tip_frame_transform); - - // setup for IK call - std::vector solution(num_joints_); - moveit_msgs::msg::MoveItErrorCodes err; - kinematics::KinematicsQueryOptions opts; - opts.return_approximate_solution = true; - if (ik_solver_->searchPositionIK(next_pose, internal_joint_state_.position, servo_params_.publish_period / 2.0, - solution, err, opts)) - { - // find the difference in joint positions that will get us to the desired pose - for (size_t i = 0; i < num_joints_; ++i) - { - delta_theta_.coeffRef(i) = solution.at(i) - internal_joint_state_.position.at(i); - } - } - else - { - RCLCPP_WARN(LOGGER, "Could not find IK solution for requested motion, got error code %d", err.val); - return false; - } - } - else - { - // no supported IK plugin, use inverse Jacobian - delta_theta_ = pseudo_inverse * delta_x; - } - - const StatusCode last_status = status_; - delta_theta_ *= velocityScalingFactorForSingularity(joint_model_group_, delta_x, svd, pseudo_inverse, - servo_params_.hard_stop_singularity_threshold, - servo_params_.lower_singularity_threshold, - servo_params_.leaving_singularity_threshold_multiplier, - current_state_, status_); - // Status will have changed if approaching singularity - if (last_status != status_) - { - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, SERVO_STATUS_CODE_MAP.at(status_)); - } - - return internalServoUpdate(delta_theta_, joint_trajectory, ServoType::CARTESIAN_SPACE); -} - -bool ServoCalcs::jointServoCalcs(const control_msgs::msg::JointJog& cmd, - trajectory_msgs::msg::JointTrajectory& joint_trajectory) -{ - // Check for nan's - if (!checkValidCommand(cmd)) - return false; - - // Apply user-defined scaling - delta_theta_ = scaleJointCommand(cmd); - - // Perform internal servo with the command - return internalServoUpdate(delta_theta_, joint_trajectory, ServoType::JOINT_SPACE); -} - -bool ServoCalcs::internalServoUpdate(Eigen::ArrayXd& delta_theta, - trajectory_msgs::msg::JointTrajectory& joint_trajectory, - const ServoType servo_type) -{ - // The order of operations here is: - // 1. apply velocity scaling for collisions (in the position domain) - // 2. low-pass filter the position command in applyJointUpdate() - // 3. calculate velocities in applyJointUpdate() - // 4. apply velocity limits - // 5. apply position limits. This is a higher priority than velocity limits, so check it last. - - // Apply collision scaling - double collision_scale = collision_velocity_scale_; - if (collision_scale > 0 && collision_scale < 1) - { - status_ = StatusCode::DECELERATE_FOR_COLLISION; - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, SERVO_STATUS_CODE_MAP.at(status_)); - } - else if (collision_scale == 0) - { - status_ = StatusCode::HALT_FOR_COLLISION; - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_ERROR_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, "Halting for collision!"); - } - delta_theta *= collision_scale; - - // Loop through joints and update them, calculate velocities, and filter - if (!applyJointUpdate(servo_params_.publish_period, delta_theta, previous_joint_state_, internal_joint_state_, - smoother_)) - { - RCLCPP_ERROR_STREAM_THROTTLE(LOGGER, *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, - "Lengths of output and increments do not match."); - return false; - } - - // Mark the lowpass filters as updated for this cycle - updated_filters_ = true; - - // Enforce SRDF velocity limits - enforceVelocityLimits(joint_model_group_, servo_params_.publish_period, internal_joint_state_, - servo_params_.override_velocity_scaling_factor); - - // Enforce SRDF position limits, might halt if needed, set prev_vel to 0 - const auto joints_to_halt = - enforcePositionLimits(internal_joint_state_, servo_params_.joint_limit_margin, joint_model_group_); - - if (!joints_to_halt.empty()) - { - std::ostringstream joint_names; - std::transform(joints_to_halt.cbegin(), joints_to_halt.cend(), std::ostream_iterator(joint_names, ""), - [](const auto& joint) { return " '" + joint->getName() + "'"; }); - - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, - "Joints" << joint_names.str() << " close to a position limit. Halting."); - - status_ = StatusCode::JOINT_BOUND; - if ((servo_type == ServoType::JOINT_SPACE && !servo_params_.halt_all_joints_in_joint_mode) || - (servo_type == ServoType::CARTESIAN_SPACE && !servo_params_.halt_all_joints_in_cartesian_mode)) - { - suddenHalt(internal_joint_state_, joints_to_halt); - } - else - { - suddenHalt(internal_joint_state_, joint_model_group_->getActiveJointModels()); - } - } - - // compose outgoing message - composeJointTrajMessage(internal_joint_state_, joint_trajectory); - - previous_joint_state_ = internal_joint_state_; - return true; -} - -void ServoCalcs::resetLowPassFilters(const sensor_msgs::msg::JointState& joint_state) -{ - smoother_->reset(joint_state.position); - updated_filters_ = true; -} - -void ServoCalcs::composeJointTrajMessage(const sensor_msgs::msg::JointState& joint_state, - trajectory_msgs::msg::JointTrajectory& joint_trajectory) -{ - // When a joint_trajectory_controller receives a new command, a stamp of 0 indicates "begin immediately" - // See http://wiki.ros.org/joint_trajectory_controller#Trajectory_replacement - joint_trajectory.header.stamp = rclcpp::Time(0); - joint_trajectory.header.frame_id = servo_params_.planning_frame; - joint_trajectory.joint_names = joint_state.name; - - trajectory_msgs::msg::JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(servo_params_.publish_period); - if (servo_params_.publish_joint_positions) - point.positions = joint_state.position; - if (servo_params_.publish_joint_velocities) - point.velocities = joint_state.velocity; - if (servo_params_.publish_joint_accelerations) - { - // I do not know of a robot that takes acceleration commands. - // However, some controllers check that this data is non-empty. - // Send all zeros, for now. - std::vector acceleration(num_joints_); - point.accelerations = acceleration; - } - joint_trajectory.points.push_back(point); -} - -void ServoCalcs::filteredHalt(trajectory_msgs::msg::JointTrajectory& joint_trajectory) -{ - // Prepare the joint trajectory message to stop the robot - joint_trajectory.points.clear(); - joint_trajectory.points.emplace_back(); - joint_trajectory.joint_names = joint_model_group_->getActiveJointModelNames(); - - // Deceleration algorithm: - // Set positions to internal_joint_state_ - // Filter - // Calculate velocities - // Check if velocities are close to zero. Round to zero, if so. - assert(internal_joint_state_.position.size() >= num_joints_); - joint_trajectory.points[0].positions = internal_joint_state_.position; - smoother_->doSmoothing(joint_trajectory.points[0].positions); - bool done_stopping = true; - if (servo_params_.publish_joint_velocities) - { - joint_trajectory.points[0].velocities = std::vector(num_joints_, 0); - for (std::size_t i = 0; i < num_joints_; ++i) - { - joint_trajectory.points[0].velocities.at(i) = - (joint_trajectory.points[0].positions.at(i) - internal_joint_state_.position.at(i)) / - servo_params_.publish_period; - // If velocity is very close to zero, round to zero - if (joint_trajectory.points[0].velocities.at(i) > STOPPED_VELOCITY_EPS) - { - done_stopping = false; - } - } - // If every joint is very close to stopped, round velocity to zero - if (done_stopping) - { - std::fill(joint_trajectory.points[0].velocities.begin(), joint_trajectory.points[0].velocities.end(), 0); - } - } - - if (servo_params_.publish_joint_accelerations) - { - joint_trajectory.points[0].accelerations = std::vector(num_joints_, 0); - for (std::size_t i = 0; i < num_joints_; ++i) - { - joint_trajectory.points[0].accelerations.at(i) = - (joint_trajectory.points[0].velocities.at(i) - internal_joint_state_.velocity.at(i)) / - servo_params_.publish_period; - } - } - - joint_trajectory.points[0].time_from_start = rclcpp::Duration::from_seconds(servo_params_.publish_period); -} - -void ServoCalcs::suddenHalt(sensor_msgs::msg::JointState& joint_state, - const std::vector& joints_to_halt) const -{ - // Set the position to the original position, and velocity to 0 for input joints - for (const auto& joint_to_halt : joints_to_halt) - { - const auto joint_it = std::find(joint_state.name.cbegin(), joint_state.name.cend(), joint_to_halt->getName()); - if (joint_it != joint_state.name.cend()) - { - const auto joint_index = std::distance(joint_state.name.cbegin(), joint_it); - joint_state.position.at(joint_index) = previous_joint_state_.position.at(joint_index); - joint_state.velocity.at(joint_index) = 0.0; - } - } -} - -bool ServoCalcs::checkValidCommand(const control_msgs::msg::JointJog& cmd) -{ - for (double velocity : cmd.velocities) - { - if (std::isnan(velocity)) - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "nan in incoming command. Skipping this datapoint."); - return false; - } - } - return true; -} - -bool ServoCalcs::checkValidCommand(const geometry_msgs::msg::TwistStamped& cmd) -{ - if (std::isnan(cmd.twist.linear.x) || std::isnan(cmd.twist.linear.y) || std::isnan(cmd.twist.linear.z) || - std::isnan(cmd.twist.angular.x) || std::isnan(cmd.twist.angular.y) || std::isnan(cmd.twist.angular.z)) - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "nan in incoming command. Skipping this datapoint."); - return false; - } - - // If incoming commands should be in the range [-1:1], check for |delta|>1 - if (servo_params_.command_in_type == "unitless") - { - if ((fabs(cmd.twist.linear.x) > 1) || (fabs(cmd.twist.linear.y) > 1) || (fabs(cmd.twist.linear.z) > 1) || - (fabs(cmd.twist.angular.x) > 1) || (fabs(cmd.twist.angular.y) > 1) || (fabs(cmd.twist.angular.z) > 1)) - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "Component of incoming command is >1. Skipping this datapoint."); - return false; - } - } - - // Check that the command frame is known - if (!cmd.header.frame_id.empty() && !current_state_->knowsFrameTransform(cmd.header.frame_id)) - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "Commanded frame '" << cmd.header.frame_id << "' is unknown, skipping this command"); - return false; - } - - return true; -} - -// Scale the incoming jog command. Returns a vector of position deltas -Eigen::VectorXd ServoCalcs::scaleCartesianCommand(const geometry_msgs::msg::TwistStamped& command) -{ - Eigen::VectorXd result(6); - result.setZero(); // Or the else case below leads to misery - - // Apply user-defined scaling if inputs are unitless [-1:1] - if (servo_params_.command_in_type == "unitless") - { - result[0] = servo_params_.scale.linear * servo_params_.publish_period * command.twist.linear.x; - result[1] = servo_params_.scale.linear * servo_params_.publish_period * command.twist.linear.y; - result[2] = servo_params_.scale.linear * servo_params_.publish_period * command.twist.linear.z; - result[3] = servo_params_.scale.rotational * servo_params_.publish_period * command.twist.angular.x; - result[4] = servo_params_.scale.rotational * servo_params_.publish_period * command.twist.angular.y; - result[5] = servo_params_.scale.rotational * servo_params_.publish_period * command.twist.angular.z; - } - // Otherwise, commands are in m/s and rad/s - else if (servo_params_.command_in_type == "speed_units") - { - result[0] = command.twist.linear.x * servo_params_.publish_period; - result[1] = command.twist.linear.y * servo_params_.publish_period; - result[2] = command.twist.linear.z * servo_params_.publish_period; - result[3] = command.twist.angular.x * servo_params_.publish_period; - result[4] = command.twist.angular.y * servo_params_.publish_period; - result[5] = command.twist.angular.z * servo_params_.publish_period; - } - else - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_ERROR_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, "Unexpected command_in_type"); - } - - return result; -} - -Eigen::VectorXd ServoCalcs::scaleJointCommand(const control_msgs::msg::JointJog& command) -{ - Eigen::VectorXd result(num_joints_); - result.setZero(); - - std::size_t c; - for (std::size_t m = 0; m < command.joint_names.size(); ++m) - { - try - { - c = joint_state_name_map_.at(command.joint_names[m]); - } - catch (const std::out_of_range& e) - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, "Ignoring joint " << command.joint_names[m]); - continue; - } - // Apply user-defined scaling if inputs are unitless [-1:1] - if (servo_params_.command_in_type == "unitless") - { - result[c] = command.velocities[m] * servo_params_.scale.joint * servo_params_.publish_period; - // Otherwise, commands are in m/s and rad/s - } - else if (servo_params_.command_in_type == "speed_units") - { - result[c] = command.velocities[m] * servo_params_.publish_period; - } - else - { - rclcpp::Clock& clock = *node_->get_clock(); - RCLCPP_ERROR_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "Unexpected command_in_type, check yaml file."); - } - } - - return result; -} - -bool ServoCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform) -{ - const std::lock_guard lock(main_loop_mutex_); - transform = tf_moveit_to_robot_cmd_frame_; - - // All zeros means the transform wasn't initialized, so return false - return !transform.matrix().isZero(0); -} - -bool ServoCalcs::getCommandFrameTransform(geometry_msgs::msg::TransformStamped& transform) -{ - const std::lock_guard lock(main_loop_mutex_); - // All zeros means the transform wasn't initialized, so return false - if (tf_moveit_to_robot_cmd_frame_.matrix().isZero(0)) - { - return false; - } - - transform = convertIsometryToTransform(tf_moveit_to_robot_cmd_frame_, servo_params_.planning_frame, - servo_params_.robot_link_command_frame); - return true; -} - -bool ServoCalcs::getEEFrameTransform(Eigen::Isometry3d& transform) -{ - const std::lock_guard lock(main_loop_mutex_); - transform = tf_moveit_to_ee_frame_; - - // All zeros means the transform wasn't initialized, so return false - return !transform.matrix().isZero(0); -} - -bool ServoCalcs::getEEFrameTransform(geometry_msgs::msg::TransformStamped& transform) -{ - const std::lock_guard lock(main_loop_mutex_); - // All zeros means the transform wasn't initialized, so return false - if (tf_moveit_to_ee_frame_.matrix().isZero(0)) - { - return false; - } - - transform = - convertIsometryToTransform(tf_moveit_to_ee_frame_, servo_params_.planning_frame, servo_params_.ee_frame_name); - return true; -} - -void ServoCalcs::twistStampedCB(const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg) -{ - const std::lock_guard lock(main_loop_mutex_); - latest_twist_stamped_ = msg; - - if (msg->header.stamp != rclcpp::Time(0.)) - latest_twist_command_stamp_ = msg->header.stamp; - - // notify that we have a new input - new_input_cmd_ = true; - input_cv_.notify_all(); -} - -void ServoCalcs::jointCmdCB(const control_msgs::msg::JointJog::ConstSharedPtr& msg) -{ - const std::lock_guard lock(main_loop_mutex_); - latest_joint_cmd_ = msg; - - if (msg->header.stamp != rclcpp::Time(0.)) - latest_joint_command_stamp_ = msg->header.stamp; - - // notify that we have a new input - new_input_cmd_ = true; - input_cv_.notify_all(); -} - -void ServoCalcs::collisionVelocityScaleCB(const std_msgs::msg::Float64::ConstSharedPtr& msg) -{ - collision_velocity_scale_ = msg->data; -} - -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 17d7c11a7e..91c6854279 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -31,20 +31,43 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -/* Title : servo_node.cpp - * Project : moveit_servo - * Created : 12/31/2018 - * Author : Andy Zelenak +/* Title : servo_node.cpp + * Project : moveit_servo + * Created : 12/31/2018 + * Author : Andy Zelenak, V Mohammed Ibrahim + * */ -#include +#include +#include -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_node"); +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_node"); +} // namespace namespace moveit_servo { + +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr ServoNode::get_node_base_interface() +{ + return node_->get_node_base_interface(); +} + +ServoNode::~ServoNode() +{ + stop_servo_ = true; + if (servo_loop_thread_.joinable()) + servo_loop_thread_.join(); +} + ServoNode::ServoNode(const rclcpp::NodeOptions& options) : node_{ std::make_shared("servo_node", options) } + , stop_servo_{ false } + , servo_paused_{ false } + , new_joint_jog_msg_{ false } + , new_twist_msg_{ false } + , new_pose_msg_{ false } { if (!options.use_intra_process_comms()) { @@ -53,111 +76,273 @@ ServoNode::ServoNode(const rclcpp::NodeOptions& options) "in the launch file"); } - // Set up services for interacting with Servo - start_servo_service_ = node_->create_service( - "~/start_servo", - [this](const std::shared_ptr& request, - const std::shared_ptr& response) { return startCB(request, response); }); - stop_servo_service_ = node_->create_service( - "~/stop_servo", - [this](const std::shared_ptr& request, - const std::shared_ptr& response) { return stopCB(request, response); }); - - // Can set robot_description name from parameters - std::string robot_description_name = "robot_description"; - node_->get_parameter_or("robot_description_name", robot_description_name, robot_description_name); - - // Get the servo parameters - auto param_listener = std::make_unique(node_, "moveit_servo"); - auto servo_parameters = param_listener->get_params(); - // Validate the parameters first. - validateParams(servo_parameters); - - // Set up planning_scene_monitor - planning_scene_monitor_ = std::make_shared( - node_, robot_description_name, "planning_scene_monitor"); - planning_scene_monitor_->startStateMonitor(servo_parameters.joint_topic); - planning_scene_monitor_->startSceneMonitor(servo_parameters.monitored_planning_scene_topic); - planning_scene_monitor_->setPlanningScenePublishingFrequency(25); - planning_scene_monitor_->getStateMonitor()->enableCopyDynamics(true); - planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, - std::string(node_->get_fully_qualified_name()) + - "/publish_planning_scene"); - // If the planning scene monitor in servo is the primary one we provide /get_planning_scene service so RViz displays - // or secondary planning scene monitors can fetch the scene, otherwise we request the planning scene from the - // primary planning scene monitor (e.g. move_group) - if (servo_parameters.is_primary_planning_scene_monitor) - { - planning_scene_monitor_->providePlanningSceneService(); + // Check if a realtime kernel is available + if (realtime_tools::has_realtime_kernel()) + { + if (realtime_tools::configure_sched_fifo(servo_params_.thread_priority)) + { + RCLCPP_INFO_STREAM(LOGGER, "Realtime kernel available, higher thread priority has been set."); + } + else + { + RCLCPP_WARN_STREAM(LOGGER, "Could not enable FIFO RT scheduling policy."); + } } else { - planning_scene_monitor_->requestPlanningSceneState(); + RCLCPP_WARN_STREAM(LOGGER, "Realtime kernel is recommended for better performance."); + } + + std::shared_ptr servo_param_listener = + std::make_shared(node_, "moveit_servo"); + + // Create Servo instance + planning_scene_monitor_ = createPlanningSceneMonitor(node_, servo_param_listener->get_params()); + servo_ = std::make_unique(node_, servo_param_listener, planning_scene_monitor_); + + servo_params_ = servo_->getParams(); + + // Create subscriber for jointjog + joint_jog_subscriber_ = node_->create_subscription( + servo_params_.joint_command_in_topic, rclcpp::SystemDefaultsQoS(), + [this](const control_msgs::msg::JointJog::ConstSharedPtr& msg) { return jointJogCallback(msg); }); + + // Create subscriber for twist + twist_subscriber_ = node_->create_subscription( + servo_params_.cartesian_command_in_topic, rclcpp::SystemDefaultsQoS(), + [this](const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg) { return twistCallback(msg); }); + + // Create subscriber for pose + pose_subscriber_ = node_->create_subscription( + servo_params_.pose_command_in_topic, rclcpp::SystemDefaultsQoS(), + [this](const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg) { return poseCallback(msg); }); + + if (servo_params_.command_out_type == "trajectory_msgs/JointTrajectory") + { + trajectory_publisher_ = node_->create_publisher( + servo_params_.command_out_topic, rclcpp::SystemDefaultsQoS()); + } + else if (servo_params_.command_out_type == "std_msgs/Float64MultiArray") + { + multi_array_publisher_ = node_->create_publisher(servo_params_.command_out_topic, + rclcpp::SystemDefaultsQoS()); } + // Create status publisher + status_publisher_ = + node_->create_publisher(servo_params_.status_topic, rclcpp::SystemDefaultsQoS()); + + // Create service to enable switching command type + switch_command_type_ = node_->create_service( + "~/switch_command_type", [this](const std::shared_ptr& request, + const std::shared_ptr& response) { + return switchCommandType(request, response); + }); + + // Create service to pause/unpause servoing + pause_servo_ = node_->create_service( + "~/pause_servo", [this](const std::shared_ptr& request, + const std::shared_ptr& response) { + return pauseServo(request, response); + }); - // Create Servo - servo_ = std::make_unique(node_, planning_scene_monitor_, std::move(param_listener)); + // Start the servoing loop + servo_loop_thread_ = std::thread(&ServoNode::servoLoop, this); } -void ServoNode::validateParams(const servo::Params& servo_params) +void ServoNode::pauseServo(const std::shared_ptr& request, + const std::shared_ptr& response) { - bool has_error = false; - if (servo_params.hard_stop_singularity_threshold <= servo_params.lower_singularity_threshold) + servo_paused_ = request->data; + response->success = (servo_paused_ == request->data); + if (servo_paused_) { - RCLCPP_ERROR(LOGGER, "Parameter 'hard_stop_singularity_threshold' " - "should be greater than 'lower_singularity_threshold.' " - "Check the parameters YAML file used to launch this node."); - has_error = true; + servo_->setCollisionChecking(false); + response->message = "Servoing disabled"; } - - if (!servo_params.publish_joint_positions && !servo_params.publish_joint_velocities && - !servo_params.publish_joint_accelerations) + else { - RCLCPP_ERROR(LOGGER, "At least one of publish_joint_positions / " - "publish_joint_velocities / " - "publish_joint_accelerations must be true. " - "Check the parameters YAML file used to launch this node."); - has_error = true; + servo_->setCollisionChecking(true); + response->message = "Servoing enabled"; } +} - if ((servo_params.command_out_type == "std_msgs/Float64MultiArray") && servo_params.publish_joint_positions && - servo_params.publish_joint_velocities) +void ServoNode::switchCommandType(const std::shared_ptr& request, + const std::shared_ptr& response) +{ + const bool is_valid = (request->command_type >= static_cast(CommandType::MIN)) && + (request->command_type <= static_cast(CommandType::MAX)); + if (is_valid) { - RCLCPP_ERROR(LOGGER, "When publishing a std_msgs/Float64MultiArray, " - "you must select positions OR velocities." - "Check the parameters YAML file used to launch this node."); - has_error = true; + servo_->setCommandType(static_cast(request->command_type)); } + else + { + RCLCPP_WARN_STREAM(LOGGER, "Unknown command type " << request->command_type << " requested"); + } + response->success = (request->command_type == static_cast(servo_->getCommandType())); +} - if (servo_params.scene_collision_proximity_threshold < servo_params.self_collision_proximity_threshold) +void ServoNode::jointJogCallback(const control_msgs::msg::JointJog::ConstSharedPtr& msg) +{ + latest_joint_jog_ = *msg; + new_joint_jog_msg_ = true; +} + +void ServoNode::twistCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr& msg) +{ + latest_twist_ = *msg; + new_twist_msg_ = true; +} + +void ServoNode::poseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& msg) +{ + latest_pose_ = *msg; + new_pose_msg_ = true; +} + +std::optional ServoNode::processJointJogCommand() +{ + std::optional next_joint_state = std::nullopt; + // Reject any other command types that had arrived simultaneously. + new_twist_msg_ = new_pose_msg_ = false; + + const bool command_stale = (node_->now() - latest_joint_jog_.header.stamp) >= + rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout); + if (!command_stale) + { + JointJogCommand command{ latest_joint_jog_.joint_names, latest_joint_jog_.velocities }; + next_joint_state = servo_->getNextJointState(command); + } + else { - RCLCPP_ERROR(LOGGER, "Parameter 'self_collision_proximity_threshold' should probably be less " - "than or equal to 'scene_collision_proximity_threshold'." - "Check the parameters YAML file used to launch this node."); - has_error = true; + auto result = servo_->smoothHalt(last_commanded_state_); + new_joint_jog_msg_ = result.first; + if (new_joint_jog_msg_) + { + next_joint_state = result.second; + RCLCPP_DEBUG_STREAM(LOGGER, "Joint jog command timed out. Halting to a stop."); + } } - if (has_error) + return next_joint_state; +} + +std::optional ServoNode::processTwistCommand() +{ + std::optional next_joint_state = std::nullopt; + + // Mark latest twist command as processed. + // Reject any other command types that had arrived simultaneously. + new_joint_jog_msg_ = new_pose_msg_ = false; + + const bool command_stale = (node_->now() - latest_twist_.header.stamp) >= + rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout); + if (!command_stale) { - throw std::runtime_error("Servo failed to initialize : Invalid parameter values"); + const Eigen::Vector velocities{ latest_twist_.twist.linear.x, latest_twist_.twist.linear.y, + latest_twist_.twist.linear.z, latest_twist_.twist.angular.x, + latest_twist_.twist.angular.y, latest_twist_.twist.angular.z }; + const TwistCommand command{ latest_twist_.header.frame_id, velocities }; + next_joint_state = servo_->getNextJointState(command); } + else + { + auto result = servo_->smoothHalt(last_commanded_state_); + new_twist_msg_ = result.first; + if (new_twist_msg_) + { + next_joint_state = result.second; + RCLCPP_INFO_STREAM(LOGGER, "Twist command timed out. Halting to a stop."); + } + } + + return next_joint_state; } -void ServoNode::startCB(const std::shared_ptr& /* unused */, - const std::shared_ptr& response) +std::optional ServoNode::processPoseCommand() { - servo_->start(); - response->success = true; + std::optional next_joint_state = std::nullopt; + + // Mark latest pose command as processed. + // Reject any other command types that had arrived simultaneously. + new_joint_jog_msg_ = new_twist_msg_ = false; + + const bool command_stale = (node_->now() - latest_pose_.header.stamp) >= + rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout); + if (!command_stale) + { + const PoseCommand command = poseFromPoseStamped(latest_pose_); + next_joint_state = servo_->getNextJointState(command); + } + else + { + auto result = servo_->smoothHalt(last_commanded_state_); + new_pose_msg_ = result.first; + if (new_pose_msg_) + { + next_joint_state = result.second; + RCLCPP_DEBUG_STREAM(LOGGER, "Pose command timed out. Halting to a stop."); + } + } + + return next_joint_state; } -void ServoNode::stopCB(const std::shared_ptr& /* unused */, - const std::shared_ptr& response) +void ServoNode::servoLoop() { - servo_->stop(); - response->success = true; + moveit_msgs::msg::ServoStatus status_msg; + std::optional next_joint_state = std::nullopt; + rclcpp::WallRate servo_frequency(1 / servo_params_.publish_period); + + while (rclcpp::ok() && !stop_servo_) + { + // Skip processing if servoing is disabled. + if (servo_paused_) + continue; + + next_joint_state = std::nullopt; + const CommandType expected_type = servo_->getCommandType(); + + if (expected_type == CommandType::JOINT_JOG && new_joint_jog_msg_) + { + next_joint_state = processJointJogCommand(); + } + else if (expected_type == CommandType::TWIST && new_twist_msg_) + { + next_joint_state = processTwistCommand(); + } + else if (expected_type == CommandType::POSE && new_pose_msg_) + { + next_joint_state = processPoseCommand(); + } + else if (new_joint_jog_msg_ || new_twist_msg_ || new_pose_msg_) + { + new_joint_jog_msg_ = new_twist_msg_ = new_pose_msg_ = false; + RCLCPP_WARN_STREAM(LOGGER, "Command type has not been set, cannot accept input"); + } + + if (next_joint_state && (servo_->getStatus() != StatusCode::INVALID)) + { + if (servo_params_.command_out_type == "trajectory_msgs/JointTrajectory") + { + trajectory_publisher_->publish(composeTrajectoryMessage(servo_->getParams(), next_joint_state.value())); + } + else if (servo_params_.command_out_type == "std_msgs/Float64MultiArray") + { + multi_array_publisher_->publish(composeMultiArrayMessage(servo_->getParams(), next_joint_state.value())); + } + last_commanded_state_ = next_joint_state.value(); + } + + status_msg.code = static_cast(servo_->getStatus()); + status_msg.message = servo_->getStatusMessage(); + status_publisher_->publish(status_msg); + + servo_frequency.sleep(); + } } + } // namespace moveit_servo -// Register the component with class_loader -#include +#include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(moveit_servo::ServoNode) diff --git a/moveit_ros/moveit_servo/src/servo_node_main.cpp b/moveit_ros/moveit_servo/src/servo_node_main.cpp deleted file mode 100644 index 81fd43227e..0000000000 --- a/moveit_ros/moveit_servo/src/servo_node_main.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause 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 the copyright holder 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 HOLDER 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. - *******************************************************************************/ - -/* Title : servo_node_main.cpp - * Project : moveit_servo - * Created : 08/18/2021 - * Author : Joe Schornak - */ - -#include - -int main(int argc, char* argv[]) -{ - rclcpp::init(argc, argv); - - rclcpp::NodeOptions options; - - auto servo_node = std::make_shared(options); - - rclcpp::spin(servo_node->get_node_base_interface()); - - rclcpp::shutdown(); -} diff --git a/moveit_ros/moveit_servo/src/teleop_demo/joystick_servo_example.cpp b/moveit_ros/moveit_servo/src/teleop_demo/joystick_servo_example.cpp deleted file mode 100644 index b28fabb94a..0000000000 --- a/moveit_ros/moveit_servo/src/teleop_demo/joystick_servo_example.cpp +++ /dev/null @@ -1,279 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, 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. - *********************************************************************/ - -/* Title : joystick_servo_example.cpp - * Project : moveit_servo - * Created : 08/07/2020 - * Author : Adam Pettinger - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#if RCLCPP_VERSION_GTE(20, 0, 0) -#include -#else -#include -#endif -#include -#include -#include -#include - -// We'll just set up parameters here -const std::string JOY_TOPIC = "/joy"; -const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds"; -const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds"; -const std::string EEF_FRAME_ID = "panda_hand"; -const std::string BASE_FRAME_ID = "panda_link0"; - -// Enums for button names -> axis/button array index -// For XBOX 1 controller -enum Axis -{ - LEFT_STICK_X = 0, - LEFT_STICK_Y = 1, - LEFT_TRIGGER = 2, - RIGHT_STICK_X = 3, - RIGHT_STICK_Y = 4, - RIGHT_TRIGGER = 5, - D_PAD_X = 6, - D_PAD_Y = 7 -}; -enum Button -{ - A = 0, - B = 1, - X = 2, - Y = 3, - LEFT_BUMPER = 4, - RIGHT_BUMPER = 5, - CHANGE_VIEW = 6, - MENU = 7, - HOME = 8, - LEFT_STICK_CLICK = 9, - RIGHT_STICK_CLICK = 10 -}; - -// Some axes have offsets (e.g. the default trigger position is 1.0 not 0) -// This will map the default values for the axes -const std::map AXIS_DEFAULTS = { { LEFT_TRIGGER, 1.0 }, { RIGHT_TRIGGER, 1.0 } }; - -// To change controls or setup a new controller, all you should to do is change the above enums and the follow 2 -// functions -/** \brief // This converts a joystick axes and buttons array to a TwistStamped or JointJog message - * @param axes The vector of continuous controller joystick axes - * @param buttons The vector of discrete controller button values - * @param twist A TwistStamped message to update in prep for publishing - * @param joint A JointJog message to update in prep for publishing - * @return return true if you want to publish a Twist, false if you want to publish a JointJog - */ -bool convertJoyToCmd(const std::vector& axes, const std::vector& buttons, - std::unique_ptr& twist, - std::unique_ptr& joint) -{ - // Give joint jogging priority because it is only buttons - // If any joint jog command is requested, we are only publishing joint commands - if (buttons[A] || buttons[B] || buttons[X] || buttons[Y] || axes[D_PAD_X] || axes[D_PAD_Y]) - { - // Map the D_PAD to the proximal joints - joint->joint_names.push_back("panda_joint1"); - joint->velocities.push_back(axes[D_PAD_X]); - joint->joint_names.push_back("panda_joint2"); - joint->velocities.push_back(axes[D_PAD_Y]); - - // Map the diamond to the distal joints - joint->joint_names.push_back("panda_joint7"); - joint->velocities.push_back(buttons[B] - buttons[X]); - joint->joint_names.push_back("panda_joint6"); - joint->velocities.push_back(buttons[Y] - buttons[A]); - return false; - } - - // The bread and butter: map buttons to twist commands - twist->twist.linear.z = axes[RIGHT_STICK_Y]; - twist->twist.linear.y = axes[RIGHT_STICK_X]; - - double lin_x_right = -0.5 * (axes[RIGHT_TRIGGER] - AXIS_DEFAULTS.at(RIGHT_TRIGGER)); - double lin_x_left = 0.5 * (axes[LEFT_TRIGGER] - AXIS_DEFAULTS.at(LEFT_TRIGGER)); - twist->twist.linear.x = lin_x_right + lin_x_left; - - twist->twist.angular.y = axes[LEFT_STICK_Y]; - twist->twist.angular.x = axes[LEFT_STICK_X]; - - double roll_positive = buttons[RIGHT_BUMPER]; - double roll_negative = -1 * (buttons[LEFT_BUMPER]); - twist->twist.angular.z = roll_positive + roll_negative; - - return true; -} - -/** \brief // This should update the frame_to_publish_ as needed for changing command frame via controller - * @param frame_name Set the command frame to this - * @param buttons The vector of discrete controller button values - */ -void updateCmdFrame(std::string& frame_name, const std::vector& buttons) -{ - if (buttons[CHANGE_VIEW] && frame_name == EEF_FRAME_ID) - { - frame_name = BASE_FRAME_ID; - } - else if (buttons[MENU] && frame_name == BASE_FRAME_ID) - { - frame_name = EEF_FRAME_ID; - } -} - -namespace moveit_servo -{ -class JoyToServoPub : public rclcpp::Node -{ -public: - JoyToServoPub(const rclcpp::NodeOptions& options) - : Node("joy_to_twist_publisher", options), frame_to_publish_(BASE_FRAME_ID) - { - // Setup pub/sub - joy_sub_ = create_subscription(JOY_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const sensor_msgs::msg::Joy::ConstSharedPtr& msg) { - return joyCB(msg); - }); - - twist_pub_ = create_publisher(TWIST_TOPIC, rclcpp::SystemDefaultsQoS()); - joint_pub_ = create_publisher(JOINT_TOPIC, rclcpp::SystemDefaultsQoS()); - collision_pub_ = create_publisher("/planning_scene", rclcpp::SystemDefaultsQoS()); - - // Create a service client to start the ServoNode - servo_start_client_ = create_client("/servo_node/start_servo"); - servo_start_client_->wait_for_service(std::chrono::seconds(1)); - servo_start_client_->async_send_request(std::make_shared()); - - // Load the collision scene asynchronously - collision_pub_thread_ = std::thread([this]() { - rclcpp::sleep_for(std::chrono::seconds(3)); - // Create collision object, in the way of servoing - moveit_msgs::msg::CollisionObject collision_object; - collision_object.header.frame_id = "panda_link0"; - collision_object.id = "box"; - - shape_msgs::msg::SolidPrimitive table_1; - table_1.type = table_1.BOX; - table_1.dimensions = { 0.4, 0.6, 0.03 }; - - geometry_msgs::msg::Pose table_1_pose; - table_1_pose.position.x = 0.6; - table_1_pose.position.y = 0.0; - table_1_pose.position.z = 0.4; - - shape_msgs::msg::SolidPrimitive table_2; - table_2.type = table_2.BOX; - table_2.dimensions = { 0.6, 0.4, 0.03 }; - - geometry_msgs::msg::Pose table_2_pose; - table_2_pose.position.x = 0.0; - table_2_pose.position.y = 0.5; - table_2_pose.position.z = 0.25; - - collision_object.primitives.push_back(table_1); - collision_object.primitive_poses.push_back(table_1_pose); - collision_object.primitives.push_back(table_2); - collision_object.primitive_poses.push_back(table_2_pose); - collision_object.operation = collision_object.ADD; - - moveit_msgs::msg::PlanningSceneWorld psw; - psw.collision_objects.push_back(collision_object); - - auto ps = std::make_unique(); - ps->world = psw; - ps->is_diff = true; - collision_pub_->publish(std::move(ps)); - }); - } - - ~JoyToServoPub() override - { - if (collision_pub_thread_.joinable()) - collision_pub_thread_.join(); - } - - void joyCB(const sensor_msgs::msg::Joy::ConstSharedPtr& msg) - { - // Create the messages we might publish - auto twist_msg = std::make_unique(); - auto joint_msg = std::make_unique(); - - // This call updates the frame for twist commands - updateCmdFrame(frame_to_publish_, msg->buttons); - - // Convert the joystick message to Twist or JointJog and publish - if (convertJoyToCmd(msg->axes, msg->buttons, twist_msg, joint_msg)) - { - // publish the TwistStamped - twist_msg->header.frame_id = frame_to_publish_; - twist_msg->header.stamp = now(); - twist_pub_->publish(std::move(twist_msg)); - } - else - { - // publish the JointJog - joint_msg->header.stamp = now(); - joint_msg->header.frame_id = "panda_link3"; - joint_pub_->publish(std::move(joint_msg)); - } - } - -private: - rclcpp::Subscription::SharedPtr joy_sub_; - rclcpp::Publisher::SharedPtr twist_pub_; - rclcpp::Publisher::SharedPtr joint_pub_; - rclcpp::Publisher::SharedPtr collision_pub_; - rclcpp::Client::SharedPtr servo_start_client_; - - std::string frame_to_publish_; - - std::thread collision_pub_thread_; -}; // class JoyToServoPub - -} // namespace moveit_servo - -// Register the component with class_loader -#include -RCLCPP_COMPONENTS_REGISTER_NODE(moveit_servo::JoyToServoPub) diff --git a/moveit_ros/moveit_servo/src/utilities.cpp b/moveit_ros/moveit_servo/src/utilities.cpp deleted file mode 100644 index 9c95a9801e..0000000000 --- a/moveit_ros/moveit_servo/src/utilities.cpp +++ /dev/null @@ -1,305 +0,0 @@ -// Copyright 2022 PickNik Inc. -// -// 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 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 HOLDER 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 : Andy Zelenak - Desc : Free functions. We keep them in a separate translation unit to reduce .o filesize - Title : utilities.cpp - Project : moveit_servo -*/ - -#include - -// Disable -Wold-style-cast because all _THROTTLE macros trigger this -// It would be too noisy to disable on a per-callsite basis -#pragma GCC diagnostic ignored "-Wold-style-cast" - -namespace moveit_servo -{ - -/** \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped **/ -geometry_msgs::msg::TransformStamped convertIsometryToTransform(const Eigen::Isometry3d& eigen_tf, - const std::string& parent_frame, - const std::string& child_frame) -{ - geometry_msgs::msg::TransformStamped output = tf2::eigenToTransform(eigen_tf); - output.header.frame_id = parent_frame; - output.child_frame_id = child_frame; - - return output; -} - -double velocityScalingFactorForSingularity(const moveit::core::JointModelGroup* joint_model_group, - const Eigen::VectorXd& commanded_twist, - const Eigen::JacobiSVD& svd, - const Eigen::MatrixXd& pseudo_inverse, - const double hard_stop_singularity_threshold, - const double lower_singularity_threshold, - const double leaving_singularity_threshold_multiplier, - const moveit::core::RobotStatePtr& current_state, StatusCode& status) -{ - double velocity_scale = 1; - std::size_t num_dimensions = commanded_twist.size(); - - // Find the direction away from nearest singularity. - // The last column of U from the SVD of the Jacobian points directly toward or away from the singularity. - // The sign can flip at any time, so we have to do some extra checking. - // Look ahead to see if the Jacobian's condition will decrease. - Eigen::VectorXd vector_toward_singularity = svd.matrixU().col(num_dimensions - 1); - - double ini_condition = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1); - - // This singular vector tends to flip direction unpredictably. See R. Bro, - // "Resolving the Sign Ambiguity in the Singular Value Decomposition". - // Look ahead to see if the Jacobian's condition will decrease in this - // direction. Start with a scaled version of the singular vector - Eigen::VectorXd delta_x(num_dimensions); - double scale = 100; - delta_x = vector_toward_singularity / scale; - - // Calculate a small change in joints - Eigen::VectorXd new_theta; - current_state->copyJointGroupPositions(joint_model_group, new_theta); - new_theta += pseudo_inverse * delta_x; - current_state->setJointGroupPositions(joint_model_group, new_theta); - Eigen::MatrixXd new_jacobian = current_state->getJacobian(joint_model_group); - - Eigen::JacobiSVD new_svd(new_jacobian); - double new_condition = new_svd.singularValues()(0) / new_svd.singularValues()(new_svd.singularValues().size() - 1); - // If new_condition < ini_condition, the singular vector does point towards a - // singularity. Otherwise, flip its direction. - if (ini_condition >= new_condition) - { - vector_toward_singularity *= -1; - } - - // If this dot product is positive, we're moving toward singularity - double dot = vector_toward_singularity.dot(commanded_twist); - // see https://github.com/ros-planning/moveit2/pull/620#issuecomment-1201418258 for visual explanation of algorithm - double upper_threshold = dot > 0 ? hard_stop_singularity_threshold : - (hard_stop_singularity_threshold - lower_singularity_threshold) * - leaving_singularity_threshold_multiplier + - lower_singularity_threshold; - if ((ini_condition > lower_singularity_threshold) && (ini_condition < hard_stop_singularity_threshold)) - { - velocity_scale = - 1. - (ini_condition - lower_singularity_threshold) / (upper_threshold - lower_singularity_threshold); - status = - dot > 0 ? StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY : StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY; - } - - // Very close to singularity, so halt. - else if (ini_condition >= upper_threshold) - { - velocity_scale = 0; - status = StatusCode::HALT_FOR_SINGULARITY; - } - - return velocity_scale; -} - -bool applyJointUpdate(const double publish_period, const Eigen::ArrayXd& delta_theta, - const sensor_msgs::msg::JointState& previous_joint_state, - sensor_msgs::msg::JointState& current_joint_state, - pluginlib::UniquePtr& smoother) -{ - // All the sizes must match - if (current_joint_state.position.size() != static_cast(delta_theta.size()) || - current_joint_state.velocity.size() != current_joint_state.position.size()) - { - return false; - } - - for (std::size_t i = 0; i < current_joint_state.position.size(); ++i) - { - // Increment joint - current_joint_state.position[i] += delta_theta[i]; - } - - smoother->doSmoothing(current_joint_state.position); - - // Calculate joint velocities - for (std::size_t i = 0; i < current_joint_state.position.size(); ++i) - { - current_joint_state.velocity[i] = - (current_joint_state.position.at(i) - previous_joint_state.position.at(i)) / publish_period; - } - - return true; -} - -void transformTwistToPlanningFrame(geometry_msgs::msg::TwistStamped& cmd, const std::string& planning_frame, - const moveit::core::RobotStatePtr& current_state) -{ - // We solve (planning_frame -> base -> cmd.header.frame_id) - // by computing (base->planning_frame)^-1 * (base->cmd.header.frame_id) - const Eigen::Isometry3d tf_moveit_to_incoming_cmd_frame = - current_state->getGlobalLinkTransform(planning_frame).inverse() * - current_state->getGlobalLinkTransform(cmd.header.frame_id); - - // Apply the transform to linear and angular velocities - // v' = R * v and w' = R * w - Eigen::Vector3d translation_vector(cmd.twist.linear.x, cmd.twist.linear.y, cmd.twist.linear.z); - Eigen::Vector3d angular_vector(cmd.twist.angular.x, cmd.twist.angular.y, cmd.twist.angular.z); - translation_vector = tf_moveit_to_incoming_cmd_frame.linear() * translation_vector; - angular_vector = tf_moveit_to_incoming_cmd_frame.linear() * angular_vector; - - // Update the values of the original command message to reflect the change in frame - cmd.header.frame_id = planning_frame; - cmd.twist.linear.x = translation_vector(0); - cmd.twist.linear.y = translation_vector(1); - cmd.twist.linear.z = translation_vector(2); - cmd.twist.angular.x = angular_vector(0); - cmd.twist.angular.y = angular_vector(1); - cmd.twist.angular.z = angular_vector(2); -} - -geometry_msgs::msg::Pose poseFromCartesianDelta(const Eigen::VectorXd& delta_x, - const Eigen::Isometry3d& base_to_tip_frame_transform) -{ - // get a transformation matrix with the desired position change & - // get a transformation matrix with desired orientation change - Eigen::Isometry3d tf_pos_delta(Eigen::Isometry3d::Identity()); - tf_pos_delta.translate(Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2])); - - Eigen::Isometry3d tf_rot_delta(Eigen::Isometry3d::Identity()); - Eigen::Quaterniond q = Eigen::AngleAxisd(delta_x[3], Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(delta_x[4], Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(delta_x[5], Eigen::Vector3d::UnitZ()); - tf_rot_delta.rotate(q); - - // Find the new tip link position without newly applied rotation - const Eigen::Isometry3d tf_no_new_rot = tf_pos_delta * base_to_tip_frame_transform; - - // we want the rotation to be applied in the requested reference frame, - // but we want the rotation to be about the EE point in space, not the origin. - // So, we need to translate to origin, rotate, then translate back - // Given T = transformation matrix from origin -> EE point in space (translation component of tf_no_new_rot) - // and T' as the opposite transformation, EE point in space -> origin (translation only) - // apply final transformation as T * R * T' * tf_no_new_rot - const Eigen::Matrix tf_translation = tf_no_new_rot.translation(); - Eigen::Isometry3d tf_neg_translation = Eigen::Isometry3d::Identity(); // T' - tf_neg_translation(0, 3) = -tf_translation(0, 0); - tf_neg_translation(1, 3) = -tf_translation(1, 0); - tf_neg_translation(2, 3) = -tf_translation(2, 0); - Eigen::Isometry3d tf_pos_translation = Eigen::Isometry3d::Identity(); // T - tf_pos_translation(0, 3) = tf_translation(0, 0); - tf_pos_translation(1, 3) = tf_translation(1, 0); - tf_pos_translation(2, 3) = tf_translation(2, 0); - - // T * R * T' * tf_no_new_rot - return tf2::toMsg(tf_pos_translation * tf_rot_delta * tf_neg_translation * tf_no_new_rot); -} - -double getVelocityScalingFactor(const moveit::core::JointModelGroup* joint_model_group, const Eigen::VectorXd& velocity) -{ - std::size_t joint_delta_index{ 0 }; - double velocity_scaling_factor{ 1.0 }; - for (const moveit::core::JointModel* joint : joint_model_group->getActiveJointModels()) - { - const auto& bounds = joint->getVariableBounds(joint->getName()); - if (bounds.velocity_bounded_ && velocity(joint_delta_index) != 0.0) - { - const double unbounded_velocity = velocity(joint_delta_index); - // Clamp each joint velocity to a joint specific [min_velocity, max_velocity] range. - const auto bounded_velocity = std::min(std::max(unbounded_velocity, bounds.min_velocity_), bounds.max_velocity_); - velocity_scaling_factor = std::min(velocity_scaling_factor, bounded_velocity / unbounded_velocity); - } - ++joint_delta_index; - } - - return velocity_scaling_factor; -} - -void enforceVelocityLimits(const moveit::core::JointModelGroup* joint_model_group, const double publish_period, - sensor_msgs::msg::JointState& joint_state, const double override_velocity_scaling_factor) -{ - // Get the velocity scaling factor - Eigen::VectorXd velocity = - Eigen::Map(joint_state.velocity.data(), joint_state.velocity.size()); - double velocity_scaling_factor = override_velocity_scaling_factor; - // if the override velocity scaling factor is approximately zero then the user is not overriding the value. - if (override_velocity_scaling_factor < 0.01) - velocity_scaling_factor = getVelocityScalingFactor(joint_model_group, velocity); - - // Take a smaller step if the velocity scaling factor is less than 1 - if (velocity_scaling_factor < 1) - { - Eigen::VectorXd velocity_residuals = (1 - velocity_scaling_factor) * velocity; - Eigen::VectorXd positions = - Eigen::Map(joint_state.position.data(), joint_state.position.size()); - positions -= velocity_residuals * publish_period; - - velocity *= velocity_scaling_factor; - // Back to sensor_msgs type - joint_state.velocity = std::vector(velocity.data(), velocity.data() + velocity.size()); - joint_state.position = std::vector(positions.data(), positions.data() + positions.size()); - } -} - -std::vector -enforcePositionLimits(sensor_msgs::msg::JointState& joint_state, const double joint_limit_margin, - const moveit::core::JointModelGroup* joint_model_group) -{ - // Halt if we're past a joint margin and joint velocity is moving even farther past - double joint_angle = 0; - std::vector joints_to_halt; - for (auto joint : joint_model_group->getActiveJointModels()) - { - for (std::size_t c = 0; c < joint_state.name.size(); ++c) - { - // Use the most recent robot joint state - if (joint_state.name[c] == joint->getName()) - { - joint_angle = joint_state.position.at(c); - break; - } - } - - if (!joint->satisfiesPositionBounds(&joint_angle, -joint_limit_margin)) - { - const std::vector& limits = joint->getVariableBoundsMsg(); - - // Joint limits are not defined for some joints. Skip them. - if (!limits.empty()) - { - // Check if pending velocity command is moving in the right direction - auto joint_itr = std::find(joint_state.name.begin(), joint_state.name.end(), joint->getName()); - auto joint_idx = std::distance(joint_state.name.begin(), joint_itr); - - if ((joint_state.velocity.at(joint_idx) < 0 && (joint_angle < (limits[0].min_position + joint_limit_margin))) || - (joint_state.velocity.at(joint_idx) > 0 && (joint_angle > (limits[0].max_position - joint_limit_margin)))) - { - joints_to_halt.push_back(joint); - } - } - } - } - return joints_to_halt; -} - -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp new file mode 100644 index 0000000000..d71e2c305f --- /dev/null +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -0,0 +1,278 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 copyright holder 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 HOLDER 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. + *******************************************************************************/ + +/* Title : command.cpp + * Project : moveit_servo + * Created : 06/04/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + */ + +#include + +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.command_processor"); +} + +namespace moveit_servo +{ + +JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, moveit::core::RobotStatePtr& robot_state, + servo::Params& servo_params) +{ + // Find the target joint position based on the commanded joint velocity + StatusCode status = StatusCode::NO_WARNING; + const auto joint_names = robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames(); + Eigen::VectorXd joint_position_delta(joint_names.size()); + Eigen::VectorXd velocities(joint_names.size()); + + velocities.setZero(); + bool names_valid = true; + + for (size_t i = 0; i < command.names.size(); i++) + { + auto it = std::find(joint_names.begin(), joint_names.end(), command.names[i]); + if (it != std::end(joint_names)) + { + velocities[std::distance(joint_names.begin(), it)] = command.velocities[i]; + } + else + { + names_valid = false; + break; + } + } + const bool velocity_valid = isValidCommand(velocities); + if (names_valid && velocity_valid) + { + joint_position_delta = velocities * servo_params.publish_period; + if (servo_params.command_in_type == "unitless") + { + joint_position_delta *= servo_params.scale.joint; + } + } + else + { + status = StatusCode::INVALID; + if (!names_valid) + { + RCLCPP_WARN_STREAM(LOGGER, "Invalid joint names in joint jog command"); + } + if (!velocity_valid) + { + RCLCPP_WARN_STREAM(LOGGER, "Invalid velocity values in joint jog command"); + } + } + return std::make_pair(status, joint_position_delta); +} + +JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, moveit::core::RobotStatePtr& robot_state, + servo::Params& servo_params) +{ + StatusCode status = StatusCode::NO_WARNING; + const int num_joints = + robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames().size(); + Eigen::VectorXd joint_position_delta(num_joints); + Eigen::VectorXd cartesian_position_delta; + + const bool valid_command = isValidCommand(command); + const bool is_planning_frame = (command.frame_id == servo_params.planning_frame); + const bool is_zero = command.velocities.isZero(); + if (!is_zero && is_planning_frame && valid_command) + { + // Compute the Cartesian position delta based on incoming command, assumed to be in m/s + cartesian_position_delta = command.velocities * servo_params.publish_period; + // This scaling is supposed to be applied to the command. + // But since it is only used here, we avoid creating a copy of the command, + // by applying the scaling to the computed Cartesian delta instead. + if (servo_params.command_in_type == "unitless") + { + cartesian_position_delta.head<3>() *= servo_params.scale.linear; + cartesian_position_delta.tail<3>() *= servo_params.scale.rotational; + } + + // Compute the required change in joint angles. + const auto delta_result = jointDeltaFromIK(cartesian_position_delta, robot_state, servo_params); + status = delta_result.first; + if (status != StatusCode::INVALID) + { + joint_position_delta = delta_result.second; + // Get velocity scaling information for singularity. + const std::pair singularity_scaling_info = + velocityScalingFactorForSingularity(robot_state, cartesian_position_delta, servo_params); + // Apply velocity scaling for singularity, if there was any scaling. + if (singularity_scaling_info.second != StatusCode::NO_WARNING) + { + status = singularity_scaling_info.second; + RCLCPP_WARN_STREAM(LOGGER, SERVO_STATUS_CODE_MAP.at(status)); + joint_position_delta *= singularity_scaling_info.first; + } + } + } + else if (is_zero) + { + joint_position_delta.setZero(); + } + else + { + status = StatusCode::INVALID; + if (!valid_command) + { + RCLCPP_WARN_STREAM(LOGGER, "Invalid twist command."); + } + if (!is_planning_frame) + { + RCLCPP_WARN_STREAM(LOGGER, + "Command frame is: " << command.frame_id << " expected: " << servo_params.planning_frame); + } + } + return std::make_pair(status, joint_position_delta); +} + +JointDeltaResult jointDeltaFromPose(const PoseCommand& command, moveit::core::RobotStatePtr& robot_state, + servo::Params& servo_params) +{ + StatusCode status = StatusCode::NO_WARNING; + const int num_joints = + robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveJointModelNames().size(); + Eigen::VectorXd joint_position_delta(num_joints); + + const bool valid_command = isValidCommand(command); + const bool is_planning_frame = command.frame_id == servo_params.planning_frame; + + if (valid_command && is_planning_frame) + { + Eigen::Vector cartesian_position_delta; + + // Compute linear and angular change needed. + const Eigen::Isometry3d ee_pose{ robot_state->getGlobalLinkTransform(servo_params.ee_frame) }; + const Eigen::Quaterniond q_current(ee_pose.rotation()), q_target(command.pose.rotation()); + const Eigen::Quaterniond q_error = q_target * q_current.inverse(); + const Eigen::AngleAxisd angle_axis_error(q_error); + + cartesian_position_delta.head<3>() = command.pose.translation() - ee_pose.translation(); + cartesian_position_delta.tail<3>() = angle_axis_error.axis() * angle_axis_error.angle(); + + // Compute the required change in joint angles. + const auto delta_result = jointDeltaFromIK(cartesian_position_delta, robot_state, servo_params); + status = delta_result.first; + if (status != StatusCode::INVALID) + { + joint_position_delta = delta_result.second; + } + } + else + { + status = StatusCode::INVALID; + if (!valid_command) + { + RCLCPP_WARN_STREAM(LOGGER, "Invalid pose command."); + } + if (!is_planning_frame) + { + RCLCPP_WARN_STREAM(LOGGER, + "Command frame is: " << command.frame_id << " expected: " << servo_params.planning_frame); + } + } + return std::make_pair(status, joint_position_delta); +} + +JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delta, + moveit::core::RobotStatePtr& robot_state, servo::Params& servo_params) +{ + const moveit::core::JointModelGroup* joint_model_group = + robot_state->getJointModelGroup(servo_params.move_group_name); + + std::vector current_joint_positions; + robot_state->copyJointGroupPositions(joint_model_group, current_joint_positions); + + Eigen::VectorXd delta_theta(current_joint_positions.size()); + StatusCode status = StatusCode::NO_WARNING; + + const kinematics::KinematicsBaseConstPtr ik_solver = joint_model_group->getSolverInstance(); + bool ik_solver_supports_group = true; + if (ik_solver) + { + ik_solver_supports_group = ik_solver->supportsGroup(joint_model_group); + if (!ik_solver_supports_group) + { + status = StatusCode::INVALID; + RCLCPP_ERROR_STREAM(LOGGER, "Loaded IK plugin does not support group " << joint_model_group->getName()); + } + } + + if (ik_solver && ik_solver_supports_group) + { + const Eigen::Isometry3d base_to_tip_frame_transform = + robot_state->getGlobalLinkTransform(ik_solver->getBaseFrame()).inverse() * + robot_state->getGlobalLinkTransform(ik_solver->getTipFrame()); + + const geometry_msgs::msg::Pose next_pose = + poseFromCartesianDelta(cartesian_position_delta, base_to_tip_frame_transform); + + // setup for IK call + std::vector solution; + moveit_msgs::msg::MoveItErrorCodes err; + kinematics::KinematicsQueryOptions opts; + opts.return_approximate_solution = true; + if (ik_solver->searchPositionIK(next_pose, current_joint_positions, servo_params.publish_period / 2.0, solution, + err, opts)) + { + // find the difference in joint positions that will get us to the desired pose + for (size_t i = 0; i < current_joint_positions.size(); ++i) + { + delta_theta[i] = solution[i] - current_joint_positions[i]; + } + } + else + { + status = StatusCode::INVALID; + RCLCPP_WARN_STREAM(LOGGER, "Could not find IK solution for requested motion, got error code " << err.val); + } + } + else + { + // Robot does not have an IK solver, use inverse Jacobian to compute IK. + const Eigen::MatrixXd jacobian = robot_state->getJacobian(joint_model_group); + const Eigen::JacobiSVD svd = + Eigen::JacobiSVD(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); + const Eigen::MatrixXd matrix_s = svd.singularValues().asDiagonal(); + const Eigen::MatrixXd pseudo_inverse = svd.matrixV() * matrix_s.inverse() * svd.matrixU().transpose(); + + delta_theta = pseudo_inverse * cartesian_position_delta; + } + + return std::make_pair(status, delta_theta); +} + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/utils/common.cpp b/moveit_ros/moveit_servo/src/utils/common.cpp new file mode 100644 index 0000000000..fe28852a69 --- /dev/null +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -0,0 +1,348 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * 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 copyright holder 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 HOLDER 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. + *******************************************************************************/ + +/* Title : utils.cpp + * Project : moveit_servo + * Created : 05/17/2023 + * Author : Andy Zelenak, V Mohammed Ibrahim + */ + +#include + +namespace +{ +// The threshold above which `override_velocity_scaling_factor` will be used instead of computing the scaling from joint bounds. +const double SCALING_OVERRIDE_THRESHOLD = 0.01; +} // namespace + +namespace moveit_servo +{ + +bool isValidCommand(const Eigen::VectorXd& command) +{ + // returns true only if there are no nan values. + return command.allFinite(); +} + +bool isValidCommand(const Eigen::Isometry3d& command) +{ + Eigen::Matrix3d identity, rotation; + identity.setIdentity(); + rotation = command.linear(); + // checks rotation, will fail if there is nan + const bool is_valid_rotation = rotation.allFinite() && identity.isApprox(rotation.inverse() * rotation); + // Command is not vald if there is Nan + const bool is_valid_translation = isValidCommand(command.translation()); + return is_valid_rotation && is_valid_translation; +} + +bool isValidCommand(const TwistCommand& command) +{ + return !command.frame_id.empty() && isValidCommand(command.velocities); +} + +bool isValidCommand(const PoseCommand& command) +{ + return !command.frame_id.empty() && isValidCommand(command.pose); +} + +geometry_msgs::msg::Pose poseFromCartesianDelta(const Eigen::VectorXd& delta_x, + const Eigen::Isometry3d& base_to_tip_frame_transform) +{ + // Get a transformation matrix with the desired position change + Eigen::Isometry3d tf_pos_delta(Eigen::Isometry3d::Identity()); + tf_pos_delta.translate(Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2])); + + // Get a transformation matrix with desired orientation change + Eigen::Isometry3d tf_rot_delta(Eigen::Isometry3d::Identity()); + const Eigen::Quaterniond q = Eigen::AngleAxisd(delta_x[3], Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(delta_x[4], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(delta_x[5], Eigen::Vector3d::UnitZ()); + tf_rot_delta.rotate(q); + + // Find the new tip link position without newly applied rotation + const Eigen::Isometry3d tf_no_new_rot = tf_pos_delta * base_to_tip_frame_transform; + + // we want the rotation to be applied in the requested reference frame, + // but we want the rotation to be about the EE point in space, not the origin. + // So, we need to translate to origin, rotate, then translate back + // Given T = transformation matrix from origin -> EE point in space (translation component of tf_no_new_rot) + // and T' as the opposite transformation, EE point in space -> origin (translation only) + // apply final transformation as T * R * T' * tf_no_new_rot + Eigen::Isometry3d tf_neg_translation = Eigen::Isometry3d::Identity(); // T' + tf_neg_translation.translation() = -1 * tf_no_new_rot.translation(); + Eigen::Isometry3d tf_pos_translation = Eigen::Isometry3d::Identity(); // T + tf_pos_translation.translation() = tf_no_new_rot.translation(); + + // T * R * T' * tf_no_new_rot + return tf2::toMsg(tf_pos_translation * tf_rot_delta * tf_neg_translation * tf_no_new_rot); +} + +trajectory_msgs::msg::JointTrajectory composeTrajectoryMessage(const servo::Params& servo_params, + const KinematicState& joint_state) +{ + // When a joint_trajectory_controller receives a new command, a stamp of 0 indicates "begin immediately" + // See http://wiki.ros.org/joint_trajectory_controller#Trajectory_replacement + + trajectory_msgs::msg::JointTrajectory joint_trajectory; + joint_trajectory.header.stamp = rclcpp::Time(0); + joint_trajectory.header.frame_id = servo_params.planning_frame; + joint_trajectory.joint_names = joint_state.joint_names; + + trajectory_msgs::msg::JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(servo_params.publish_period); + + // Set the fields of trajectory point based on which fields are requested. + // Some controllers check that acceleration data is non-empty, even if accelerations are not used + // Send all zeros (joint_state.accelerations is a vector of all zeros). + point.positions = servo_params.publish_joint_positions ? joint_state.positions : point.positions; + point.velocities = servo_params.publish_joint_velocities ? joint_state.velocities : point.velocities; + point.accelerations = servo_params.publish_joint_accelerations ? joint_state.accelerations : point.accelerations; + + joint_trajectory.points.push_back(point); + return joint_trajectory; +} + +std_msgs::msg::Float64MultiArray composeMultiArrayMessage(const servo::Params& servo_params, + const KinematicState& joint_state) +{ + std_msgs::msg::Float64MultiArray multi_array; + if (servo_params.publish_joint_positions) + { + multi_array.data = joint_state.positions; + } + else if (servo_params.publish_joint_velocities) + { + multi_array.data = joint_state.velocities; + } + + return multi_array; +} + +std::pair velocityScalingFactorForSingularity(const moveit::core::RobotStatePtr& robot_state, + const Eigen::VectorXd& target_delta_x, + const servo::Params& servo_params) +{ + // We need to send information back about if we are halting, moving away or towards the singularity. + StatusCode servo_status = StatusCode::NO_WARNING; + + const moveit::core::JointModelGroup* joint_model_group = + robot_state->getJointModelGroup(servo_params.move_group_name); + + // Get the thresholds. + const double lower_singularity_threshold = servo_params.lower_singularity_threshold; + const double hard_stop_singularity_threshold = servo_params.hard_stop_singularity_threshold; + const double leaving_singularity_threshold_multiplier = servo_params.leaving_singularity_threshold_multiplier; + + // Get size of total controllable dimensions. + const size_t dims = target_delta_x.size(); + + // Get the current Jacobian and compute SVD + const Eigen::JacobiSVD current_svd = Eigen::JacobiSVD( + robot_state->getJacobian(joint_model_group), Eigen::ComputeThinU | Eigen::ComputeThinV); + const Eigen::MatrixXd matrix_s = current_svd.singularValues().asDiagonal(); + + // Compute pseudo inverse + const Eigen::MatrixXd pseudo_inverse = current_svd.matrixV() * matrix_s.inverse() * current_svd.matrixU().transpose(); + + // Get the singular vector corresponding to least singular value. + // This vector represents the least responsive dimension. By convention this is the last column of the matrix U. + // The sign of the singular vector from result of SVD is not reliable, so we need to do extra checking to make sure of + // the sign. See R. Bro, "Resolving the Sign Ambiguity in the Singular Value Decomposition". + Eigen::VectorXd vector_towards_singularity = current_svd.matrixU().col(dims - 1); + + // Compute the current condition number. The ratio of max and min singular values. + // By convention these are the first and last element of the diagonal. + const double current_condition_number = current_svd.singularValues()(0) / current_svd.singularValues()(dims - 1); + + // Take a small step in the direction of vector_towards_singularity + const Eigen::VectorXd delta_x = vector_towards_singularity * servo_params.singularity_step_scale; + + // Compute the new joint angles if we take the small step delta_x + Eigen::VectorXd next_joint_angles; + robot_state->copyJointGroupPositions(joint_model_group, next_joint_angles); + next_joint_angles += pseudo_inverse * delta_x; + + // Compute the Jacobian SVD for the new robot state. + robot_state->setJointGroupPositions(joint_model_group, next_joint_angles); + const Eigen::JacobiSVD next_svd = Eigen::JacobiSVD( + robot_state->getJacobian(joint_model_group), Eigen::ComputeThinU | Eigen::ComputeThinV); + + // Compute condition number for the new Jacobian. + const double next_condition_number = next_svd.singularValues()(0) / next_svd.singularValues()(dims - 1); + + // If the condition number has increased, we are moving towards singularity and the direction of the + // vector_towards_singularity is correct. If the condition number has decreased, it means the sign of + // vector_towards_singularity needs to be flipped. + if (next_condition_number <= current_condition_number) + { + vector_towards_singularity *= -1; + } + + // Double check the direction using dot product. + const bool moving_towards_singularity = vector_towards_singularity.dot(target_delta_x) > 0; + + // Compute upper condition variable threshold based on if we are moving towards or away from singularity. + // See https://github.com/ros-planning/moveit2/pull/620#issuecomment-1201418258 for visual explanation. + double upper_threshold; + if (moving_towards_singularity) + { + upper_threshold = hard_stop_singularity_threshold; + } + else + { + const double threshold_size = (hard_stop_singularity_threshold - lower_singularity_threshold); + upper_threshold = lower_singularity_threshold + (threshold_size * leaving_singularity_threshold_multiplier); + } + + // Compute the scale based on the current condition number. + double velocity_scale = 1.0; + const bool is_above_lower_limit = current_condition_number > lower_singularity_threshold; + const bool is_below_hard_stop_limit = current_condition_number < hard_stop_singularity_threshold; + if (is_above_lower_limit && is_below_hard_stop_limit) + { + velocity_scale -= + (current_condition_number - lower_singularity_threshold) / (upper_threshold - lower_singularity_threshold); + + servo_status = moving_towards_singularity ? StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY : + StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY; + } + // If condition number has crossed hard stop limit, halt the robot. + else if (!is_below_hard_stop_limit) + { + servo_status = StatusCode::HALT_FOR_SINGULARITY; + velocity_scale = 0.0; + } + + return std::make_pair(velocity_scale, servo_status); +} + +double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities, + const moveit::core::JointBoundsVector& joint_bounds, double scaling_override) +{ + // If override value is close to zero, user is not overriding the scaling + if (scaling_override < SCALING_OVERRIDE_THRESHOLD) + { + scaling_override = 1.0; // Set to no scaling. + double bounded_vel; + std::vector velocity_scaling_factors; // The allowable fraction of computed veclocity + + for (size_t i = 0; i < joint_bounds.size(); i++) + { + const auto joint_bound = (joint_bounds[i])->front(); + if (joint_bound.velocity_bounded_ && velocities(i) != 0.0) + { + // Find the ratio of clamped velocity to original velocity + bounded_vel = std::clamp(velocities(i), joint_bound.min_velocity_, joint_bound.max_velocity_); + velocity_scaling_factors.push_back(bounded_vel / velocities(i)); + } + } + // Find the lowest scaling factor, this helps preserve Cartesian motion. + scaling_override = velocity_scaling_factors.empty() ? + scaling_override : + *std::min_element(velocity_scaling_factors.begin(), velocity_scaling_factors.end()); + } + + return scaling_override; +} + +std::vector jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const moveit::core::JointBoundsVector& joint_bounds, double margin) +{ + std::vector joint_idxs_to_halt; + for (size_t i = 0; i < joint_bounds.size(); i++) + { + const auto joint_bound = (joint_bounds[i])->front(); + if (joint_bound.position_bounded_) + { + const bool negative_bound = velocities[i] < 0 && positions[i] < (joint_bound.min_position_ + margin); + const bool positive_bound = velocities[i] > 0 && positions[i] > (joint_bound.max_position_ - margin); + if (negative_bound || positive_bound) + { + joint_idxs_to_halt.push_back(i); + } + } + } + return joint_idxs_to_halt; +} + +/** \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped **/ +geometry_msgs::msg::TransformStamped convertIsometryToTransform(const Eigen::Isometry3d& eigen_tf, + const std::string& parent_frame, + const std::string& child_frame) +{ + geometry_msgs::msg::TransformStamped output = tf2::eigenToTransform(eigen_tf); + output.header.frame_id = parent_frame; + output.child_frame_id = child_frame; + return output; +} + +PoseCommand poseFromPoseStamped(const geometry_msgs::msg::PoseStamped& msg) +{ + PoseCommand command; + command.frame_id = msg.header.frame_id; + + const Eigen::Vector3d translation(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z); + const Eigen::Quaterniond rotation(msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, + msg.pose.orientation.z); + + command.pose = Eigen::Isometry3d::Identity(); + command.pose.translate(translation); + command.pose.linear() = rotation.normalized().toRotationMatrix(); + + return command; +} + +planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, + const servo::Params& servo_params) +{ + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor; + // Can set robot_description name from parameters + std::string robot_description_name = "robot_description"; + node->get_parameter_or("robot_description_name", robot_description_name, robot_description_name); + // Set up planning_scene_monitor + planning_scene_monitor = std::make_shared(node, robot_description_name, + "planning_scene_monitor"); + + planning_scene_monitor->startStateMonitor(servo_params.joint_topic); + planning_scene_monitor->startSceneMonitor(servo_params.monitored_planning_scene_topic); + planning_scene_monitor->setPlanningScenePublishingFrequency(25); + planning_scene_monitor->getStateMonitor()->enableCopyDynamics(true); + planning_scene_monitor->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, + std::string(node->get_fully_qualified_name()) + + "/publish_planning_scene"); + + return planning_scene_monitor; +} + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/test/config/collision_start_positions.yaml b/moveit_ros/moveit_servo/test/config/collision_start_positions.yaml deleted file mode 100644 index a0c8afdfe4..0000000000 --- a/moveit_ros/moveit_servo/test/config/collision_start_positions.yaml +++ /dev/null @@ -1,16 +0,0 @@ -initial_positions: - panda_joint1: 0.0 - panda_joint2: -0.785 - panda_joint3: 0.0 - panda_joint4: -2.9 - panda_joint5: 0.0 - panda_joint6: 1.1 - panda_joint7: 0.785 -initial_velocities: - panda_joint1: 0.0 - panda_joint2: 0.0 - panda_joint3: 0.0 - panda_joint4: 0.0 - panda_joint5: 0.0 - panda_joint6: 0.0 - panda_joint7: 0.0 diff --git a/moveit_ros/moveit_servo/test/config/pose_tracking_settings.yaml b/moveit_ros/moveit_servo/test/config/pose_tracking_settings.yaml deleted file mode 100644 index 5c2a4889a2..0000000000 --- a/moveit_ros/moveit_servo/test/config/pose_tracking_settings.yaml +++ /dev/null @@ -1,22 +0,0 @@ -################################# -# PID parameters for pose seeking -################################# - - -# Maximum value of error integral for all PID controllers -windup_limit: 0.05 - -# PID gains -x_proportional_gain: 1.5 -y_proportional_gain: 1.5 -z_proportional_gain: 1.5 -x_integral_gain: 0.0 -y_integral_gain: 0.0 -z_integral_gain: 0.0 -x_derivative_gain: 0.0 -y_derivative_gain: 0.0 -z_derivative_gain: 0.0 - -angular_proportional_gain: 0.5 -angular_integral_gain: 0.0 -angular_derivative_gain: 0.0 diff --git a/moveit_ros/moveit_servo/test/config/servo_settings.yaml b/moveit_ros/moveit_servo/test/config/servo_settings.yaml deleted file mode 100644 index 007aa8002f..0000000000 --- a/moveit_ros/moveit_servo/test/config/servo_settings.yaml +++ /dev/null @@ -1,58 +0,0 @@ -############################################### -# Modify all parameters related to servoing here -############################################### - -## Properties of incoming commands -command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s -scale: - # Scale parameters are only used if command_in_type=="unitless" - linear: 0.003 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.006 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. - # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. - joint: 0.01 - -## Properties of outgoing commands -low_latency_mode: false # Set this to true to tie the output rate to the input rate -publish_period: 0.01 # 1/Nominal publish rate [seconds] - -# What type of topic does your robot driver expect? -# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController or JointGroupPositionController) -# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots) -command_out_type: trajectory_msgs/JointTrajectory - -# What to publish? Can save some bandwidth as most robots only require positions or velocities -publish_joint_positions: true -publish_joint_velocities: false -publish_joint_accelerations: false - -## Plugins for smoothing outgoing commands -smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" - -## MoveIt properties -move_group_name: panda_arm # Often 'manipulator' or 'arm' -planning_frame: panda_link0 # The MoveIt planning frame. Often 'panda_link0' or 'world' - -## Other frames -ee_frame_name: panda_link7 # The name of the end effector link, used to return the EE pose -robot_link_command_frame: panda_link0 # commands must be given in the frame of a robot link. Usually either the base or end effector - -## Stopping behaviour -incoming_command_timeout: 1.0 # Stop servoing if X seconds elapse without a new command - -## Configure handling of singularities and joint limits -lower_singularity_threshold: 30.0 # Start decelerating when the condition number hits this (close to singularity) -hard_stop_singularity_threshold: 90.0 # Stop when the condition number hits this -joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. - -## Topic names -cartesian_command_in_topic: servo_node/delta_twist_cmds # Topic for incoming Cartesian twist commands -joint_command_in_topic: servo_node/delta_joint_cmds # Topic for incoming joint angle commands -joint_topic: joint_states -status_topic: servo_node/status # Publish status to this topic -command_out_topic: servo_node/command # Publish outgoing commands here - -## Collision checking for the entire robot body -check_collisions: true # Check collisions? -collision_check_rate: 5.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. -self_collision_proximity_threshold: 0.01 # Start decelerating when a collision is this far [m] -scene_collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m] diff --git a/moveit_ros/moveit_servo/test/launch/test_servo_collision.test.py b/moveit_ros/moveit_servo/test/launch/test_servo_collision.test.py deleted file mode 100644 index 2a6d28a78e..0000000000 --- a/moveit_ros/moveit_servo/test/launch/test_servo_collision.test.py +++ /dev/null @@ -1,27 +0,0 @@ -import launch_testing -import os -import sys -import unittest - -sys.path.append(os.path.dirname(__file__)) -from servo_launch_test_common import generate_servo_test_description - - -def generate_test_description(): - return generate_servo_test_description( - gtest_name="test_servo_collision", - start_position_path="../config/collision_start_positions.yaml", - ) - - -class TestGTestWaitForCompletion(unittest.TestCase): - # Waits for test to complete, then waits a bit to make sure result files are generated - def test_gtest_run_complete(self, servo_gtest): - self.proc_info.assertWaitForShutdown(servo_gtest, timeout=4000.0) - - -@launch_testing.post_shutdown_test() -class TestGTestProcessPostShutdown(unittest.TestCase): - # Checks if the test has been completed with acceptable exit codes (successful codes) - def test_gtest_pass(self, proc_info, servo_gtest): - launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest) diff --git a/moveit_ros/moveit_servo/test/launch/test_servo_integration.test.py b/moveit_ros/moveit_servo/test/launch/test_servo_integration.test.py deleted file mode 100644 index 769c256f5b..0000000000 --- a/moveit_ros/moveit_servo/test/launch/test_servo_integration.test.py +++ /dev/null @@ -1,24 +0,0 @@ -import launch_testing -import os -import sys -import unittest - -sys.path.append(os.path.dirname(__file__)) -from servo_launch_test_common import generate_servo_test_description - - -def generate_test_description(): - return generate_servo_test_description(gtest_name="test_servo_integration") - - -class TestGTestWaitForCompletion(unittest.TestCase): - # Waits for test to complete, then waits a bit to make sure result files are generated - def test_gtest_run_complete(self, servo_gtest): - self.proc_info.assertWaitForShutdown(servo_gtest, timeout=4000.0) - - -@launch_testing.post_shutdown_test() -class TestGTestProcessPostShutdown(unittest.TestCase): - # Checks if the test has been completed with acceptable exit codes (successful codes) - def test_gtest_pass(self, proc_info, servo_gtest): - launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest) diff --git a/moveit_ros/moveit_servo/test/pose_tracking_test.cpp b/moveit_ros/moveit_servo/test/pose_tracking_test.cpp deleted file mode 100644 index cf7f2d7eb1..0000000000 --- a/moveit_ros/moveit_servo/test/pose_tracking_test.cpp +++ /dev/null @@ -1,220 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, 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: Andy Zelenak - Desc: Test of tracking toward a pose -*/ - -// C++ -#include -#include - -// ROS -#include - -// Testing -#include - -// Servo -#include -#include - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.pose_tracking_test"); - -static constexpr double TRANSLATION_TOLERANCE = 0.01; // meters -static constexpr double ROTATION_TOLERANCE = 0.1; // quaternion - -namespace moveit_servo -{ -class PoseTrackingFixture : public ::testing::Test -{ -public: - void SetUp() override - { - executor_->add_node(node_); - executor_thread_ = std::thread([this]() { this->executor_->spin(); }); - - auto servo_param_listener = std::make_unique(node_, "moveit_servo"); - servo_parameters_ = servo_param_listener->get_params(); - - // store test constants as shared pointer to constant struct - { - auto test_parameters = std::make_shared(); - test_parameters->publish_hz = 2.0 / servo_parameters_.incoming_command_timeout; - test_parameters->publish_period = 1.0 / test_parameters->publish_hz; - test_parameters->timeout_iterations = 10 * test_parameters->publish_hz; - test_parameters_ = test_parameters; - } - - // Load the planning scene monitor - if (!planning_scene_monitor_->getPlanningScene()) - { - RCLCPP_ERROR_STREAM(LOGGER, "Error in setting up the PlanningSceneMonitor."); - exit(EXIT_FAILURE); - } - - planning_scene_monitor_->providePlanningSceneService(); - planning_scene_monitor_->startSceneMonitor(); - planning_scene_monitor_->startWorldGeometryMonitor( - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, - false /* skip octomap monitor */); - planning_scene_monitor_->startStateMonitor(servo_parameters_.joint_topic); - planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE); - - // Wait for Planning Scene Monitor to setup - if (!planning_scene_monitor_->waitForCurrentRobotState(node_->now(), 5.0 /* seconds */)) - { - RCLCPP_ERROR_STREAM(LOGGER, "Error waiting for current robot state in PlanningSceneMonitor."); - exit(EXIT_FAILURE); - } - - tracker_ = - std::make_shared(node_, std::move(servo_param_listener), planning_scene_monitor_); - - // Tolerance for pose seeking - translation_tolerance_ << TRANSLATION_TOLERANCE, TRANSLATION_TOLERANCE, TRANSLATION_TOLERANCE; - } - - PoseTrackingFixture() - : node_(std::make_shared("pose_tracking_testing")) - , executor_(std::make_shared()) - , planning_scene_monitor_( - std::make_shared(node_, "robot_description")) - { - // Init ROS interfaces - // Publishers - target_pose_pub_ = node_->create_publisher("target_pose", 1); - } - - void TearDown() override - { - executor_->cancel(); - if (executor_thread_.joinable()) - executor_thread_.join(); - } - -protected: - rclcpp::Node::SharedPtr node_; - rclcpp::Executor::SharedPtr executor_; - std::thread executor_thread_; - - servo::Params servo_parameters_; - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - - struct TestParameters - { - double publish_hz; - double publish_period; - double timeout_iterations; - }; - std::shared_ptr test_parameters_; - - moveit_servo::PoseTrackingPtr tracker_; - - Eigen::Vector3d translation_tolerance_; - rclcpp::Publisher::SharedPtr target_pose_pub_; -}; // class PoseTrackingFixture - -// Check for commands going out to ros_control -TEST_F(PoseTrackingFixture, OutgoingMsgTest) -{ - // halt Servoing when first msg to ros_control is received - // and test some conditions - trajectory_msgs::msg::JointTrajectory last_received_msg; - std::function traj_callback = - [&/* this */](const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr& msg) { - EXPECT_EQ(msg->header.frame_id, "panda_link0"); - - // Exact joint positions may vary based on IK solver - // so check that the EE transform is as expected - moveit::core::RobotStatePtr temp_state(planning_scene_monitor_->getStateMonitor()->getCurrentState()); - const std::string group_name = "panda_arm"; - // copyJointGroupPositions can't take a const vector, make a copy - std::vector positions(msg->points[0].positions); - temp_state->copyJointGroupPositions(group_name, positions); - Eigen::Isometry3d hand_tf = temp_state->getFrameTransform("panda_hand"); - - moveit::core::RobotStatePtr test_state(planning_scene_monitor_->getStateMonitor()->getCurrentState()); - std::vector test_positions = { 0, -0.785, 0, -2.360, 0, 1.571, 0.758 }; - test_state->copyJointGroupPositions(group_name, test_positions); - Eigen::Isometry3d test_hand_tf = test_state->getFrameTransform("panda_hand"); - - double precision = 0.02; // Hilbert-Schmidt norm - EXPECT_TRUE(test_hand_tf.isApprox(hand_tf, precision)); - - this->tracker_->stopMotion(); - return; - }; - auto traj_sub = node_->create_subscription( - "/panda_arm_controller/joint_trajectory", rclcpp::SystemDefaultsQoS(), traj_callback); - - geometry_msgs::msg::PoseStamped target_pose; - target_pose.header.frame_id = "panda_link4"; - target_pose.header.stamp = node_->now(); - target_pose.pose.position.x = 0.2; - target_pose.pose.position.y = 0.2; - target_pose.pose.position.z = 0.2; - target_pose.pose.orientation.w = 1; - - // Republish the target pose in a new thread, as if the target is moving - std::thread target_pub_thread([&] { - size_t msg_count = 0; - rclcpp::WallRate loop_rate(50); - while (++msg_count < 100) - { - target_pose_pub_->publish(target_pose); - loop_rate.sleep(); - } - }); - - // resetTargetPose() can be used to clear the target pose and wait for a new one, e.g. when moving between multiple waypoints - tracker_->resetTargetPose(); - - tracker_->moveToPose(translation_tolerance_, ROTATION_TOLERANCE, 1 /* target pose timeout */); - - target_pub_thread.join(); -} - -} // namespace moveit_servo - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} diff --git a/moveit_ros/moveit_servo/test/publish_fake_jog_commands.cpp b/moveit_ros/moveit_servo/test/publish_fake_jog_commands.cpp deleted file mode 100644 index b9a0c779c5..0000000000 --- a/moveit_ros/moveit_servo/test/publish_fake_jog_commands.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, 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 : Adam Pettinger - Desc : Simple node to publish commands to the Servo demo - Title : test_servo_parameters.cpp - Project : moveit_servo - Created : 07/16/2020 -*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#if RCLCPP_VERSION_GTE(20, 0, 0) -#include -#else -#include -#endif -#include -#include - -using namespace std::chrono_literals; - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.publish_fake_jog_commands.cpp"); - - // ROS objects - rclcpp::NodeOptions node_options; - auto node = std::make_shared("publish_fake_jog_commands", node_options); - - // Call the start service to init and start the servo component - rclcpp::Client::SharedPtr servo_start_client = - node->create_client("/servo_node/start_servo"); - servo_start_client->wait_for_service(1s); - servo_start_client->async_send_request(std::make_shared()); - - // Create a publisher for publishing the jog commands - auto pub = node->create_publisher("/servo_node/delta_twist_cmds", 10); - std::weak_ptr::type> captured_pub = pub; - std::weak_ptr::type> captured_node = node; - auto callback = [captured_pub, captured_node]() -> void { - auto pub_ptr = captured_pub.lock(); - auto node_ptr = captured_node.lock(); - if (!pub_ptr || !node_ptr) - { - return; - } - auto msg = std::make_unique(); - msg->header.stamp = node_ptr->now(); - msg->header.frame_id = "panda_link0"; - msg->twist.linear.x = 0.3; - msg->twist.angular.z = 0.5; - pub_ptr->publish(std::move(msg)); - }; - auto timer = node->create_wall_timer(50ms, callback); - - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/moveit_ros/moveit_servo/test/servo_calcs_unit_tests.cpp b/moveit_ros/moveit_servo/test/servo_calcs_unit_tests.cpp deleted file mode 100644 index 5a8262ca35..0000000000 --- a/moveit_ros/moveit_servo/test/servo_calcs_unit_tests.cpp +++ /dev/null @@ -1,176 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, 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: Tyler Weaver, Andy Zelenak - Desc: Unit tests -*/ - -#include - -#include -#include -#include -#include - -namespace -{ -constexpr double PUBLISH_PERIOD = 0.01; - -void checkVelocityLimits(const moveit::core::JointModelGroup* joint_model_group, const Eigen::ArrayXd& velocity) -{ - std::size_t joint_index{ 0 }; - for (const moveit::core::JointModel* joint : joint_model_group->getActiveJointModels()) - { - const auto& bounds = joint->getVariableBounds(joint->getName()); - - if (bounds.velocity_bounded_) - { - EXPECT_GE(velocity(joint_index), bounds.min_velocity_) << "Joint " << joint_index << " violates velocity limit"; - EXPECT_LE(velocity(joint_index), bounds.max_velocity_) << "Joint " << joint_index << " violates velocity limit"; - } - ++joint_index; - } -} - -class ServoCalcsUnitTests : public testing::Test -{ -protected: - void SetUp() override - { - robot_model_ = moveit::core::loadTestingRobotModel("panda"); - joint_model_group_ = robot_model_->getJointModelGroup("panda_arm"); - } - - moveit::core::RobotModelPtr robot_model_; - const moveit::core::JointModelGroup* joint_model_group_; -}; - -} // namespace - -TEST_F(ServoCalcsUnitTests, VelocitiesTooFast) -{ - // Request velocities that are too fast - std::vector joint_position{ 0, 0, 0, 0, 0, 0, 0 }; - std::vector joint_velocity{ 0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 }; - sensor_msgs::msg::JointState joint_state; - joint_state.position = joint_position; - joint_state.velocity = joint_velocity; - - moveit_servo::enforceVelocityLimits(joint_model_group_, PUBLISH_PERIOD, joint_state); - - Eigen::ArrayXd eigen_velocity = - Eigen::Map(joint_state.velocity.data(), joint_state.velocity.size()); - checkVelocityLimits(joint_model_group_, eigen_velocity); -} - -TEST_F(ServoCalcsUnitTests, NegativeVelocitiesTooFast) -{ - // Negative velocities exceeding the limit - std::vector joint_position{ 0, 0, 0, 0, 0, 0, 0 }; - std::vector joint_velocity{ 0, -1.0, -2.0, -3.0, -4.0, -5.0, -6.0 }; - sensor_msgs::msg::JointState joint_state; - joint_state.position = joint_position; - joint_state.velocity = joint_velocity; - - moveit_servo::enforceVelocityLimits(joint_model_group_, PUBLISH_PERIOD, joint_state); - - Eigen::ArrayXd eigen_velocity = - Eigen::Map(joint_state.velocity.data(), joint_state.velocity.size()); - checkVelocityLimits(joint_model_group_, eigen_velocity); -} - -TEST_F(ServoCalcsUnitTests, AcceptableJointVelocities) -{ - // Final test with joint velocities that are acceptable - std::vector joint_position{ 0, 0, 0, 0, 0, 0, 0 }; - std::vector joint_velocity{ 0, 0.001, 0.001, -0.001, 0.001, 0.001, 0.001 }; - sensor_msgs::msg::JointState joint_state; - joint_state.position = joint_position; - joint_state.velocity = joint_velocity; - - moveit_servo::enforceVelocityLimits(joint_model_group_, PUBLISH_PERIOD, joint_state); - - Eigen::ArrayXd eigen_velocity = - Eigen::Map(joint_state.velocity.data(), joint_state.velocity.size()); - checkVelocityLimits(joint_model_group_, eigen_velocity); -} - -TEST_F(ServoCalcsUnitTests, SingularityScaling) -{ - // If we are at a singularity, we should halt - Eigen::VectorXd commanded_twist(6); - commanded_twist << 1, 0, 0, 0, 0, 0; - - // Start near a singularity - std::shared_ptr robot_state = std::make_shared(robot_model_); - robot_state->setToDefaultValues(); - robot_state->setVariablePosition("panda_joint1", 0.221); - robot_state->setVariablePosition("panda_joint2", 0.530); - robot_state->setVariablePosition("panda_joint3", -0.231); - robot_state->setVariablePosition("panda_joint4", -0.920); - robot_state->setVariablePosition("panda_joint5", 0.117); - robot_state->setVariablePosition("panda_joint6", 1.439); - robot_state->setVariablePosition("panda_joint7", -1.286); - - Eigen::MatrixXd jacobian = robot_state->getJacobian(joint_model_group_); - - Eigen::JacobiSVD svd = - Eigen::JacobiSVD(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); - Eigen::MatrixXd matrix_s = svd.singularValues().asDiagonal(); - Eigen::MatrixXd pseudo_inverse = svd.matrixV() * matrix_s.inverse() * svd.matrixU().transpose(); - - // Use very low thresholds to ensure they are triggered - double hard_stop_singularity_threshold = 2; - double lower_singularity_threshold = 1; - double leaving_singularity_threshold_multiplier = 2; - - rclcpp::Clock clock; - moveit_servo::StatusCode status; - - double scaling_factor = - moveit_servo::velocityScalingFactorForSingularity(joint_model_group_, commanded_twist, svd, pseudo_inverse, - hard_stop_singularity_threshold, lower_singularity_threshold, - leaving_singularity_threshold_multiplier, robot_state, status); - - EXPECT_EQ(scaling_factor, 0); -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} diff --git a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp deleted file mode 100644 index 86e260def9..0000000000 --- a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp +++ /dev/null @@ -1,426 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, 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. - *********************************************************************/ - -/* Title : servo_launch_test_common.hpp - * Project : moveit_servo - * Created : 08/03/2020 - * Author : Adam Pettinger - */ - -// C++ -#include - -// ROS -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Testing -#include - -// Servo -#include -// Auto-generated -#include - -#pragma once - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_launch_test_common.hpp"); - -namespace moveit_servo -{ -class ServoFixture : public ::testing::Test -{ -public: - void SetUp() override - { - executor_->add_node(node_); - executor_thread_ = std::thread([this]() { executor_->spin(); }); - } - - ServoFixture() - : node_(std::make_shared("servo_testing")) - , executor_(std::make_shared()) - { - auto servo_param_listener = std::make_unique(node_, "moveit_servo"); - servo_parameters_ = servo_param_listener->get_params(); - // store test constants as shared pointer to constant struct - { - auto test_parameters = std::make_shared(); - test_parameters->publish_hz = 2.0 / servo_parameters_.incoming_command_timeout; - test_parameters->publish_period = 1.0 / test_parameters->publish_hz; - test_parameters->timeout_iterations = 50 * test_parameters->publish_hz; - test_parameters->servo_node_name = "/servo_node"; - test_parameters_ = test_parameters; - } - - // Init ROS interfaces - // Publishers - pub_twist_cmd_ = node_->create_publisher( - resolveServoTopicName(servo_parameters_.cartesian_command_in_topic), rclcpp::SystemDefaultsQoS()); - pub_joint_cmd_ = node_->create_publisher( - resolveServoTopicName(servo_parameters_.joint_command_in_topic), rclcpp::SystemDefaultsQoS()); - } - - void TearDown() override - { - // If the stop client isn't null, we set it up and likely started the Servo. Stop it. - // Otherwise the Servo is still running when another test starts... - if (!client_servo_stop_) - { - stop(); - } - executor_->cancel(); - if (executor_thread_.joinable()) - executor_thread_.join(); - } - - std::string resolveServoTopicName(std::string topic_name) - { - if (topic_name.at(0) == '~') - { - return topic_name.replace(0, 1, test_parameters_->servo_node_name); - } - else - { - return topic_name; - } - } - - // Set up for callbacks (so they aren't run for EVERY test) - bool setupStartClient() - { - // Start client - client_servo_start_ = node_->create_client(resolveServoTopicName("~/start_servo")); - while (!client_servo_start_->service_is_ready()) - { - if (!rclcpp::ok()) - { - RCLCPP_ERROR(LOGGER, "Interrupted while waiting for the service. Exiting."); - return false; - } - RCLCPP_INFO(LOGGER, "client_servo_start_ service not available, waiting again..."); - rclcpp::sleep_for(std::chrono::milliseconds(500)); - } - - // If we setup the start client, also setup the stop client... - client_servo_stop_ = node_->create_client(resolveServoTopicName("~/stop_servo")); - while (!client_servo_stop_->service_is_ready()) - { - if (!rclcpp::ok()) - { - RCLCPP_ERROR(LOGGER, "Interrupted while waiting for the service. Exiting."); - return false; - } - RCLCPP_INFO(LOGGER, "client_servo_stop_ service not available, waiting again..."); - rclcpp::sleep_for(std::chrono::milliseconds(500)); - } - - // Status sub (we need this to check that we've started / stopped) - sub_servo_status_ = node_->create_subscription( - resolveServoTopicName(servo_parameters_.status_topic), rclcpp::SystemDefaultsQoS(), - [this](const std_msgs::msg::Int8::ConstSharedPtr& msg) { return statusCB(msg); }); - return true; - } - - bool setupCollisionScaleSub() - { - sub_collision_scale_ = node_->create_subscription( - resolveServoTopicName("~/collision_velocity_scale"), rclcpp::SystemDefaultsQoS(), - [this](const std_msgs::msg::Float64::ConstSharedPtr& msg) { return collisionScaleCB(msg); }); - return true; - } - - bool setupCommandSub(const std::string& command_type) - { - if (command_type == "trajectory_msgs/JointTrajectory") - { - sub_trajectory_cmd_output_ = node_->create_subscription( - resolveServoTopicName(servo_parameters_.command_out_topic), rclcpp::SystemDefaultsQoS(), - [this](const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr& msg) { return trajectoryCommandCB(msg); }); - return true; - } - else if (command_type == "std_msgs/Float64MultiArray") - { - sub_array_cmd_output_ = node_->create_subscription( - resolveServoTopicName(servo_parameters_.command_out_topic), rclcpp::SystemDefaultsQoS(), - [this](const std_msgs::msg::Float64MultiArray::ConstSharedPtr& msg) { return arrayCommandCB(msg); }); - return true; - } - else - { - RCLCPP_ERROR(LOGGER, "Invalid command type for Servo output command"); - return false; - } - } - - bool setupJointStateSub() - { - sub_joint_state_ = node_->create_subscription( - resolveServoTopicName(servo_parameters_.joint_topic), rclcpp::SystemDefaultsQoS(), - [this](const sensor_msgs::msg::JointState::ConstSharedPtr& msg) { return jointStateCB(msg); }); - return true; - } - - void statusCB(const std_msgs::msg::Int8::ConstSharedPtr& msg) - { - const std::lock_guard lock(latest_state_mutex_); - ++num_status_; - latest_status_ = static_cast(msg->data); - if (latest_status_ == status_tracking_code_) - status_seen_ = true; - } - - void collisionScaleCB(const std_msgs::msg::Float64::ConstSharedPtr& msg) - { - const std::lock_guard lock(latest_state_mutex_); - ++num_collision_scale_; - latest_collision_scale_ = msg->data; - } - - void jointStateCB(const sensor_msgs::msg::JointState::ConstSharedPtr& msg) - { - const std::lock_guard lock(latest_state_mutex_); - ++num_joint_state_; - latest_joint_state_ = msg; - } - - void trajectoryCommandCB(const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr& msg) - { - const std::lock_guard lock(latest_state_mutex_); - ++num_commands_; - latest_traj_cmd_ = msg; - } - - void arrayCommandCB(const std_msgs::msg::Float64MultiArray::ConstSharedPtr& msg) - { - const std::lock_guard lock(latest_state_mutex_); - ++num_commands_; - latest_array_cmd_ = msg; - } - - StatusCode getLatestStatus() - { - const std::lock_guard lock(latest_state_mutex_); - return latest_status_; - } - - size_t getNumStatus() - { - const std::lock_guard lock(latest_state_mutex_); - return num_status_; - } - - void resetNumStatus() - { - const std::lock_guard lock(latest_state_mutex_); - num_status_ = 0; - } - - double getLatestCollisionScale() - { - const std::lock_guard lock(latest_state_mutex_); - return latest_collision_scale_; - } - - size_t getNumCollisionScale() - { - const std::lock_guard lock(latest_state_mutex_); - return num_collision_scale_; - } - - void resetNumCollisionScale() - { - const std::lock_guard lock(latest_state_mutex_); - num_collision_scale_ = 0; - } - - sensor_msgs::msg::JointState getLatestJointState() - { - const std::lock_guard lock(latest_state_mutex_); - return *latest_joint_state_; - } - - size_t getNumJointState() - { - const std::lock_guard lock(latest_state_mutex_); - return num_joint_state_; - } - - void resetNumJointState() - { - const std::lock_guard lock(latest_state_mutex_); - num_joint_state_ = 0; - } - - trajectory_msgs::msg::JointTrajectory getLatestTrajCommand() - { - const std::lock_guard lock(latest_state_mutex_); - return *latest_traj_cmd_; - } - - std_msgs::msg::Float64MultiArray getLatestArrayCommand() - { - const std::lock_guard lock(latest_state_mutex_); - return *latest_array_cmd_; - } - - size_t getNumCommands() - { - const std::lock_guard lock(latest_state_mutex_); - return num_commands_; - } - - void resetNumCommands() - { - const std::lock_guard lock(latest_state_mutex_); - num_commands_ = 0; - } - - void watchForStatus(StatusCode code_to_watch_for) - { - const std::lock_guard lock(latest_state_mutex_); - status_seen_ = false; - status_tracking_code_ = code_to_watch_for; - } - - bool sawTrackedStatus() - { - const std::lock_guard lock(latest_state_mutex_); - return status_seen_; - } - - bool start() - { - auto time_start = node_->now(); - auto start_result = client_servo_start_->async_send_request(std::make_shared()); - if (!start_result.get()->success) - { - RCLCPP_ERROR(LOGGER, "Error returned form service call to start servo"); - return false; - } - RCLCPP_INFO_STREAM(LOGGER, "Wait for start servo: " << (node_->now() - time_start).seconds()); - - // Test that status messages start - rclcpp::WallRate publish_loop_rate(test_parameters_->publish_hz); - time_start = node_->now(); - auto num_statuses_start = getNumStatus(); - size_t iterations = 0; - while (getNumStatus() == num_statuses_start && iterations++ < test_parameters_->timeout_iterations) - { - publish_loop_rate.sleep(); - } - - RCLCPP_INFO_STREAM(LOGGER, "Wait for status num increasing: " << (node_->now() - time_start).seconds()); - - if (iterations >= test_parameters_->timeout_iterations) - { - RCLCPP_ERROR(LOGGER, "Timeout waiting for status num increasing"); - return false; - } - - return getNumStatus() > num_statuses_start; - } - - bool stop() - { - auto time_start = node_->now(); - auto stop_result = client_servo_stop_->async_send_request(std::make_shared()); - if (!stop_result.get()->success) - { - RCLCPP_ERROR(LOGGER, "Error returned form service call to stop servo"); - return false; - } - return true; - } - -protected: - rclcpp::Node::SharedPtr node_; - rclcpp::Executor::SharedPtr executor_; - std::thread executor_thread_; - servo::Params servo_parameters_; - - struct TestParameters - { - double publish_hz; - double publish_period; - double timeout_iterations; - std::string servo_node_name; - }; - std::shared_ptr test_parameters_; - - // ROS interfaces - - // Service Clients - rclcpp::Client::SharedPtr client_servo_start_; - rclcpp::Client::SharedPtr client_servo_stop_; - - // Publishers - rclcpp::Publisher::SharedPtr pub_twist_cmd_; - rclcpp::Publisher::SharedPtr pub_joint_cmd_; - - // Subscribers - rclcpp::Subscription::SharedPtr sub_servo_status_; - rclcpp::Subscription::SharedPtr sub_collision_scale_; - rclcpp::Subscription::SharedPtr sub_joint_state_; - rclcpp::Subscription::SharedPtr sub_trajectory_cmd_output_; - rclcpp::Subscription::SharedPtr sub_array_cmd_output_; - - // Subscription counting and data - size_t num_status_; - StatusCode latest_status_ = StatusCode::NO_WARNING; - - size_t num_collision_scale_; - double latest_collision_scale_; - - size_t num_joint_state_; - sensor_msgs::msg::JointState::ConstSharedPtr latest_joint_state_; - - size_t num_commands_; - trajectory_msgs::msg::JointTrajectory::ConstSharedPtr latest_traj_cmd_; - std_msgs::msg::Float64MultiArray::ConstSharedPtr latest_array_cmd_; - - bool status_seen_; - StatusCode status_tracking_code_ = StatusCode::NO_WARNING; - - mutable std::mutex latest_state_mutex_; -}; // class ServoFixture - -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/test/test_servo_collision.cpp b/moveit_ros/moveit_servo/test/test_servo_collision.cpp deleted file mode 100644 index 1bb13065c0..0000000000 --- a/moveit_ros/moveit_servo/test/test_servo_collision.cpp +++ /dev/null @@ -1,155 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, 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. - *********************************************************************/ - -/* Title : test_servo_collision.cpp - * Project : moveit_servo - * Created : 08/03/2020 - * Author : Adam Pettinger - */ - -#include "servo_launch_test_common.hpp" -#include - -namespace moveit_servo -{ -TEST_F(ServoFixture, SelfCollision) -{ - auto log_time_start = node_->now(); - ASSERT_TRUE(setupStartClient()); - auto log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Setup time: " << (log_time_end - log_time_start).seconds()); - - // Start Servo - ASSERT_TRUE(start()); - EXPECT_EQ(latest_status_, moveit_servo::StatusCode::NO_WARNING); - - // Look for DECELERATE_FOR_COLLISION status - watchForStatus(moveit_servo::StatusCode::DECELERATE_FOR_COLLISION); - - // Publish some joint jog commands that will bring us to collision - rclcpp::WallRate publish_loop_rate(test_parameters_->publish_hz); - log_time_start = node_->now(); - size_t iterations = 0; - while (!sawTrackedStatus() && iterations++ < test_parameters_->timeout_iterations) - { - auto msg = std::make_unique(); - msg->header.stamp = node_->now(); - msg->header.frame_id = "panda_link3"; - msg->joint_names.push_back("panda_joint4"); - msg->velocities.push_back(-1.0); - pub_joint_cmd_->publish(std::move(msg)); - publish_loop_rate.sleep(); - } - - EXPECT_LT(iterations, test_parameters_->timeout_iterations); - log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Wait for collision: " << (log_time_end - log_time_start).seconds()); -} - -TEST_F(ServoFixture, ExternalCollision) -{ - // NOTE: This test is meant to be run after the SelfCollision test - // It assumes the position is where the robot ends in SelfCollision test - - auto log_time_start = node_->now(); - ASSERT_TRUE(setupStartClient()); - auto log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Setup time: " << (log_time_end - log_time_start).seconds()); - - // Start Servo - ASSERT_TRUE(start()); - - // Create collision object, in the way of servoing - moveit_msgs::msg::CollisionObject collision_object; - collision_object.header.frame_id = "panda_link0"; - collision_object.id = "box"; - - shape_msgs::msg::SolidPrimitive box; - box.type = box.BOX; - box.dimensions = { 0.1, 0.4, 0.1 }; - - geometry_msgs::msg::Pose box_pose; - box_pose.position.x = 0.4; - box_pose.position.y = 0.0; - box_pose.position.z = 0.4; - - collision_object.primitives.push_back(box); - collision_object.primitive_poses.push_back(box_pose); - collision_object.operation = collision_object.ADD; - - moveit_msgs::msg::PlanningSceneWorld psw; - psw.collision_objects.push_back(collision_object); - - moveit_msgs::msg::PlanningScene ps; - ps.is_diff = true; - ps.world = psw; - - // Publish the collision object to the planning scene - auto scene_pub = node_->create_publisher("/planning_scene", 10); - scene_pub->publish(ps); - - // Look for DECELERATE_FOR_COLLISION status - watchForStatus(moveit_servo::StatusCode::DECELERATE_FOR_COLLISION); - - // Now publish twist commands that collide with the box - rclcpp::WallRate publish_loop_rate(test_parameters_->publish_hz); - log_time_start = node_->now(); - size_t iterations = 0; - while (!sawTrackedStatus() && iterations++ < test_parameters_->timeout_iterations) - { - auto msg = std::make_unique(); - msg->header.stamp = node_->now(); - msg->twist.linear.x = 0.2; - pub_twist_cmd_->publish(std::move(msg)); - publish_loop_rate.sleep(); - } - - EXPECT_LT(iterations, test_parameters_->timeout_iterations); - log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Wait for collision: " << (log_time_end - log_time_start).seconds()); -} - -} // namespace moveit_servo - -int main(int argc, char** argv) -{ - // It is important we init ros before google test because we are going to - // create a node during the google test init. - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - - int ret = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return ret; -} diff --git a/moveit_ros/moveit_servo/test/test_servo_interface.cpp b/moveit_ros/moveit_servo/test/test_servo_interface.cpp deleted file mode 100644 index f64451e6cd..0000000000 --- a/moveit_ros/moveit_servo/test/test_servo_interface.cpp +++ /dev/null @@ -1,197 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, 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: Dave Coleman and Tyler Weaver - Desc: Test for the C++ interface to moveit_servo -*/ - -#include "servo_launch_test_common.hpp" - -using namespace std::chrono_literals; - -namespace moveit_servo -{ -TEST_F(ServoFixture, SendTwistStampedTest) -{ - auto log_time_start = node_->now(); - ASSERT_TRUE(setupStartClient()); - ASSERT_TRUE(setupCommandSub(servo_parameters_.command_out_type)); - auto log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Setup time: " << (log_time_end - log_time_start).seconds()); - - // Start Servo - ASSERT_TRUE(start()); - EXPECT_EQ(latest_status_, moveit_servo::StatusCode::NO_WARNING); - - // We want to count the number of commands Servo publishes, we need timing - auto time_start = node_->now(); - - // Publish N messages with some time between, ensure it's less than the timeout for Servo - rclcpp::WallRate publish_loop_rate(test_parameters_->publish_hz); - size_t num_commands = 30; - resetNumCommands(); - for (size_t i = 0; i < num_commands && rclcpp::ok(); ++i) - { - auto msg = std::make_unique(); - msg->header.stamp = node_->now(); - msg->twist.linear.x = 0.1; - msg->twist.angular.z = 0.5; - pub_twist_cmd_->publish(std::move(msg)); - publish_loop_rate.sleep(); - } - - // Capture the time and number of received messages - auto time_end = node_->now(); - auto num_received = getNumCommands(); - - // Compare actual number received to expected number - auto num_expected = (time_end - time_start).seconds() / servo_parameters_.publish_period; - RCLCPP_INFO_STREAM(LOGGER, "Wait publish messages: " << (time_end - time_start).seconds()); - - EXPECT_GT(num_received, 0.5 * num_expected); - EXPECT_LT(num_received, 1.5 * num_expected); -} - -TEST_F(ServoFixture, SendJointServoTest) -{ - auto log_time_start = node_->now(); - ASSERT_TRUE(setupStartClient()); - ASSERT_TRUE(setupCommandSub(servo_parameters_.command_out_type)); - auto log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Setup time: " << (log_time_end - log_time_start).seconds()); - - // Start Servo - ASSERT_TRUE(start()); - EXPECT_EQ(latest_status_, moveit_servo::StatusCode::NO_WARNING); - - // We want to count the number of commands Servo publishes, we need timing - auto time_start = node_->now(); - - // Publish N messages with some time between, ensure it's less than the timeout for Servo - rclcpp::WallRate publish_loop_rate(test_parameters_->publish_hz); - size_t num_commands = 30; - resetNumCommands(); - for (size_t i = 0; i < num_commands && rclcpp::ok(); ++i) - { - auto msg = std::make_unique(); - msg->header.stamp = node_->now(); - msg->header.frame_id = "panda_link3"; - msg->joint_names.push_back("panda_joint1"); - msg->velocities.push_back(0.1); - pub_joint_cmd_->publish(std::move(msg)); - publish_loop_rate.sleep(); - } - - // Capture the time and number of received messages - auto time_end = node_->now(); - auto num_received = getNumCommands(); - - // Compare actual number received to expected number - auto num_expected = (time_end - time_start).seconds() / servo_parameters_.publish_period; - log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Wait publish messages: " << (time_end - time_start).seconds()); - - EXPECT_GT(num_received, 0.5 * num_expected); - EXPECT_LT(num_received, 1.5 * num_expected); - - // Now let's test the Servo input going stale - // We expect the command we were publishing above to continue for a while, then - // to continually receive Servo output, but with 0 velocity/delta_position - log_time_start = node_->now(); - - // Allow the last command to go stale and measure the output position - const int sleep_time = 1.5 * 1000 * servo_parameters_.incoming_command_timeout; - rclcpp::sleep_for(std::chrono::milliseconds(sleep_time)); - double joint_position_first = getLatestTrajCommand().points[0].positions[0]; - - // Now if we sleep a bit longer and check again, it should be the same - rclcpp::sleep_for(std::chrono::milliseconds(sleep_time)); - double joint_position_later = getLatestTrajCommand().points[0].positions[0]; - EXPECT_NEAR(joint_position_first, joint_position_later, 0.001); - - log_time_end = node_->now(); - RCLCPP_INFO_STREAM(LOGGER, "Wait stale command: " << (log_time_end - log_time_start).seconds()); -} - -TEST_F(ServoFixture, DynamicParameterTest) -{ - ASSERT_TRUE(setupStartClient()); - - // Start Servo - ASSERT_TRUE(start()); - EXPECT_EQ(latest_status_, moveit_servo::StatusCode::NO_WARNING); - - auto set_parameters_client = - node_->create_client(test_parameters_->servo_node_name + "/set_parameters"); - - while (!set_parameters_client->service_is_ready()) - { - if (!rclcpp::ok()) - { - ASSERT_TRUE(false) << "Interrupted while waiting for the service. Exiting."; - } - RCLCPP_INFO(LOGGER, "set_parameters_client service not yet available, waiting again..."); - rclcpp::sleep_for(500ms); - } - - // Test changing dynamic parameter - auto request = std::make_shared(); - - // This is a dynamic parameter - request->parameters.push_back( - rclcpp::Parameter("moveit_servo.robot_link_command_frame", "panda_link4").to_parameter_msg()); - - // This is not even a parameter that is declared (it should fail) - request->parameters.push_back(rclcpp::Parameter("moveit_servo.not_set_parameter", 1.0).to_parameter_msg()); - - auto set_parameter_response = set_parameters_client->async_send_request(request).get(); - - // The first one should have succeeded - ASSERT_TRUE(set_parameter_response->results[0].successful) << set_parameter_response->results[0].reason; - - // This parameter should fail to set. - ASSERT_FALSE(set_parameter_response->results[1].successful) - << "`not_set_parameter` is not a parameter and should fail to be set"; -} -} // namespace moveit_servo - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - - int ret = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return ret; -} diff --git a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py b/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py similarity index 60% rename from moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py rename to moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py index 002817cf51..ea6c730428 100644 --- a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py +++ b/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py @@ -1,5 +1,6 @@ import os import launch +import unittest import launch_ros import launch_testing from ament_index_python.packages import get_package_share_directory @@ -7,35 +8,23 @@ from launch_param_builder import ParameterBuilder -def generate_servo_test_description( - *args, - gtest_name: launch.some_substitutions_type.SomeSubstitutionsType, - start_position_path: launch.some_substitutions_type.SomeSubstitutionsType = "" -): - # Get parameters using the demo config file - servo_params = { - "moveit_servo": ParameterBuilder("moveit_servo") - .yaml("config/panda_simulated_config.yaml") - .to_dict() - } - - # Get URDF and SRDF - if start_position_path: - initial_positions_file = os.path.join( - os.path.dirname(__file__), start_position_path - ) - robot_description_mappings = {"initial_positions_file": initial_positions_file} - else: - robot_description_mappings = None - +def generate_test_description(): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") - .robot_description( - file_path="config/panda.urdf.xacro", mappings=robot_description_mappings - ) + .robot_description(file_path="config/panda.urdf.xacro") .to_moveit_configs() ) + # Get parameters for the Servo node + servo_params = { + "moveit_servo_test": ParameterBuilder("moveit_servo") + .yaml("config/test_config_panda.yaml") + .to_dict() + } + + # This filter parameter should be >1. Increase it for greater smoothing but slower motion. + low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + # ros2_control using FakeSystem as hardware ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), @@ -69,7 +58,7 @@ def generate_servo_test_description( # Component nodes for tf and Servo test_container = launch_ros.actions.ComposableNodeContainer( - name="test_servo_integration_container", + name="servo_integration_tests_container", namespace="/", package="rclcpp_components", executable="component_container_mt", @@ -90,48 +79,20 @@ def generate_servo_test_description( output="screen", ) - servo_node = launch_ros.actions.Node( - package="moveit_servo", - executable="servo_node_main", - output="screen", + servo_gtest = launch_ros.actions.Node( + executable=launch.substitutions.PathJoinSubstitution( + [ + launch.substitutions.LaunchConfiguration("test_binary_dir"), + "moveit_servo_cpp_integration_test", + ] + ), parameters=[ servo_params, + low_pass_filter_coeff, moveit_config.robot_description, moveit_config.robot_description_semantic, - moveit_config.joint_limits, moveit_config.robot_description_kinematics, ], - ) - - # NOTE: The smoothing plugin doesn't seem to be accessible from containers - # servo_container = ComposableNodeContainer( - # name="servo_container", - # namespace="/", - # package="rclcpp_components", - # executable="component_container_mt", - # composable_node_descriptions=[ - # ComposableNode( - # package="moveit_servo", - # plugin="moveit_servo::ServoNode", - # name="servo_node", - # parameters=[ - # servo_params, - # robot_description, - # robot_description_semantic, - # joint_limits_yaml, - # ], - # ), - # ], - # output="screen", - # ) - - # Unknown how to set timeout - # https://github.com/ros2/launch/issues/466 - servo_gtest = launch_ros.actions.Node( - executable=launch.substitutions.PathJoinSubstitution( - [launch.substitutions.LaunchConfiguration("test_binary_dir"), gtest_name] - ), - parameters=[servo_params], output="screen", ) @@ -145,14 +106,23 @@ def generate_servo_test_description( ros2_control_node, joint_state_broadcaster_spawner, panda_arm_controller_spawner, - servo_node, test_container, launch.actions.TimerAction(period=2.0, actions=[servo_gtest]), launch_testing.actions.ReadyToTest(), ] ), { - "servo_node": servo_node, - "test_container": test_container, "servo_gtest": servo_gtest, - "ros2_control_node": ros2_control_node, } + + +class TestGTestWaitForCompletion(unittest.TestCase): + # Waits for test to complete, then waits a bit to make sure result files are generated + def test_gtest_run_complete(self, servo_gtest): + self.proc_info.assertWaitForShutdown(servo_gtest, timeout=4000.0) + + +@launch_testing.post_shutdown_test() +class TestGTestProcessPostShutdown(unittest.TestCase): + # Checks if the test has been completed with acceptable exit codes (successful codes) + def test_gtest_pass(self, proc_info, servo_gtest): + launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest) diff --git a/moveit_ros/moveit_servo/tests/launch/servo_ros_integration.test.py b/moveit_ros/moveit_servo/tests/launch/servo_ros_integration.test.py new file mode 100644 index 0000000000..94519096ca --- /dev/null +++ b/moveit_ros/moveit_servo/tests/launch/servo_ros_integration.test.py @@ -0,0 +1,160 @@ +import os +import launch +import unittest +import launch_ros +import launch_testing +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder +from launch_param_builder import ParameterBuilder +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration + + +def generate_test_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() + ) + + # Launch Servo as a standalone node or as a "node component" for better latency/efficiency + launch_as_standalone_node = LaunchConfiguration( + "launch_as_standalone_node", default="true" + ) + + # Get parameters for the Servo node + servo_params = { + "moveit_servo": ParameterBuilder("moveit_servo") + .yaml("config/test_config_panda.yaml") + .to_dict() + } + + # This filter parameter should be >1. Increase it for greater smoothing but slower motion. + low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="screen", + ) + + joint_state_broadcaster_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager-timeout", + "300", + "--controller-manager", + "/controller_manager", + ], + output="screen", + ) + + panda_arm_controller_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + # Launch as much as possible in components + container = launch_ros.actions.ComposableNodeContainer( + name="moveit_servo_demo_container", + namespace="/", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + # Example of launching Servo as a node component + # Launching as a node component makes ROS 2 intraprocess communication more efficient. + launch_ros.descriptions.ComposableNode( + package="moveit_servo", + plugin="moveit_servo::ServoNode", + name="servo_node", + parameters=[ + servo_params, + low_pass_filter_coeff, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + condition=UnlessCondition(launch_as_standalone_node), + ), + launch_ros.descriptions.ComposableNode( + package="robot_state_publisher", + plugin="robot_state_publisher::RobotStatePublisher", + name="robot_state_publisher", + parameters=[moveit_config.robot_description], + ), + launch_ros.descriptions.ComposableNode( + package="tf2_ros", + plugin="tf2_ros::StaticTransformBroadcasterNode", + name="static_tf2_broadcaster", + parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}], + ), + ], + output="screen", + ) + # Launch a standalone Servo node. + # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC + servo_node = launch_ros.actions.Node( + package="moveit_servo", + executable="servo_node", + name="servo_node", + parameters=[ + servo_params, + low_pass_filter_coeff, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + output="screen", + condition=IfCondition(launch_as_standalone_node), + ) + + servo_gtest = launch_ros.actions.Node( + executable=launch.substitutions.PathJoinSubstitution( + [ + launch.substitutions.LaunchConfiguration("test_binary_dir"), + "moveit_servo_ros_integration_test", + ] + ), + output="screen", + ) + + return launch.LaunchDescription( + [ + servo_node, + container, + launch.actions.TimerAction(period=3.0, actions=[ros2_control_node]), + launch.actions.TimerAction( + period=5.0, actions=[joint_state_broadcaster_spawner] + ), + launch.actions.TimerAction( + period=7.0, actions=[panda_arm_controller_spawner] + ), + launch.actions.TimerAction(period=9.0, actions=[servo_gtest]), + launch_testing.actions.ReadyToTest(), + ] + ), { + "servo_gtest": servo_gtest, + } + + +class TestGTestWaitForCompletion(unittest.TestCase): + # Waits for test to complete, then waits a bit to make sure result files are generated + def test_gtest_run_complete(self, servo_gtest): + self.proc_info.assertWaitForShutdown(servo_gtest, timeout=4000.0) + + +@launch_testing.post_shutdown_test() +class TestGTestProcessPostShutdown(unittest.TestCase): + # Checks if the test has been completed with acceptable exit codes (successful codes) + def test_gtest_pass(self, proc_info, servo_gtest): + launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest) diff --git a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py b/moveit_ros/moveit_servo/tests/launch/servo_utils.test.py similarity index 74% rename from moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py rename to moveit_ros/moveit_servo/tests/launch/servo_utils.test.py index f96d7523a1..a656655334 100644 --- a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py +++ b/moveit_ros/moveit_servo/tests/launch/servo_utils.test.py @@ -1,31 +1,30 @@ import os -import pytest import launch +import unittest import launch_ros import launch_testing -import unittest from ament_index_python.packages import get_package_share_directory -from launch_param_builder import ParameterBuilder from moveit_configs_utils import MoveItConfigsBuilder +from launch_param_builder import ParameterBuilder -def generate_servo_test_description( - *args, gtest_name: launch.some_substitutions_type.SomeSubstitutionsType -): +def generate_test_description(): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") .to_moveit_configs() ) - # Get parameters for the Pose Tracking and Servo nodes + # Get parameters for the Servo node servo_params = { - "moveit_servo": ParameterBuilder("moveit_servo") - .yaml("config/pose_tracking_settings.yaml") - .yaml("config/panda_simulated_config_pose_tracking.yaml") + "moveit_servo_test": ParameterBuilder("moveit_servo") + .yaml("config/panda_simulated_config.yaml") .to_dict() } + # This filter parameter should be >1. Increase it for greater smoothing but slower motion. + low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + # ros2_control using FakeSystem as hardware ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), @@ -59,7 +58,7 @@ def generate_servo_test_description( # Component nodes for tf and Servo test_container = launch_ros.actions.ComposableNodeContainer( - name="test_pose_tracking_container", + name="servo_utils_tests_container", namespace="/", package="rclcpp_components", executable="component_container_mt", @@ -80,13 +79,19 @@ def generate_servo_test_description( output="screen", ) - pose_tracking_gtest = launch_ros.actions.Node( + servo_gtest = launch_ros.actions.Node( executable=launch.substitutions.PathJoinSubstitution( - [launch.substitutions.LaunchConfiguration("test_binary_dir"), gtest_name] + [ + launch.substitutions.LaunchConfiguration("test_binary_dir"), + "moveit_servo_utils_test", + ] ), parameters=[ - moveit_config.to_dict(), servo_params, + low_pass_filter_coeff, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, ], output="screen", ) @@ -102,28 +107,22 @@ def generate_servo_test_description( joint_state_broadcaster_spawner, panda_arm_controller_spawner, test_container, - launch.actions.TimerAction(period=2.0, actions=[pose_tracking_gtest]), + launch.actions.TimerAction(period=2.0, actions=[servo_gtest]), launch_testing.actions.ReadyToTest(), ] ), { - "test_container": test_container, - "servo_gtest": pose_tracking_gtest, - "ros2_control_node": ros2_control_node, + "servo_gtest": servo_gtest, } -def generate_test_description(): - return generate_servo_test_description(gtest_name="test_servo_pose_tracking") - - -class TestGTestProcessActive(unittest.TestCase): +class TestGTestWaitForCompletion(unittest.TestCase): + # Waits for test to complete, then waits a bit to make sure result files are generated def test_gtest_run_complete(self, servo_gtest): self.proc_info.assertWaitForShutdown(servo_gtest, timeout=4000.0) @launch_testing.post_shutdown_test() class TestGTestProcessPostShutdown(unittest.TestCase): - def test_gtest_pass( - self, proc_info, test_container, servo_gtest, ros2_control_node - ): + # Checks if the test has been completed with acceptable exit codes (successful codes) + def test_gtest_pass(self, proc_info, servo_gtest): launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_server.h b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp similarity index 53% rename from moveit_ros/moveit_servo/include/moveit_servo/servo_server.h rename to moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp index f16e132039..55fff52865 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_server.h +++ b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2020, PickNik Inc. + * Copyright (c) 2023, PickNik Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,20 +32,43 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Title : servo_server.h - * Project : moveit_servo - * Created : 07/13/2020 - * Author : Adam Pettinger +/* Title : servo_cpp_fixture.hpp + * Project : moveit_servo + * Created : 07/13/2020 + * Author : Adam Pettinger, V Mohammed Ibrahim + * + * Description : Resources used by servo c++ integration tests */ -#pragma once +#include +#include +#include +#include +#include +#include + +class ServoCppFixture : public testing::Test +{ +protected: + ServoCppFixture() + { + // Create a node to be given to Servo. + servo_test_node_ = std::make_shared("moveit_servo_test"); -#pragma message("Header `servo_server.h` is deprecated, please use `servo_node.h`") + // Create a Servo object for testing. + const std::string servo_param_namespace = "moveit_servo_test"; + servo_param_listener_ = std::make_shared(servo_test_node_, servo_param_namespace); + servo_params_ = servo_param_listener_->get_params(); -#include + planning_scene_monitor_ = moveit_servo::createPlanningSceneMonitor(servo_test_node_, servo_params_); -namespace moveit_servo -{ -using ServoServer [[deprecated("use ServoNode from servo_node.h")]] = ServoNode; + servo_test_instance_ = + std::make_shared(servo_test_node_, servo_param_listener_, planning_scene_monitor_); + } -} // namespace moveit_servo + std::shared_ptr servo_test_node_; + std::shared_ptr servo_param_listener_; + servo::Params servo_params_; + std::shared_ptr servo_test_instance_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; +}; diff --git a/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp new file mode 100644 index 0000000000..585c15eae4 --- /dev/null +++ b/moveit_ros/moveit_servo/tests/servo_ros_fixture.hpp @@ -0,0 +1,168 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, 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 : V Mohammed Ibrahim + Desc : Resources used by servo ROS integration tests + Title : servo_ros_fixture.hpp + Project : moveit_servo + Created : 07/23/2023 +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class ServoRosFixture : public testing::Test +{ +protected: + void statusCallback(const moveit_msgs::msg::ServoStatus::ConstSharedPtr& msg) + { + status_ = static_cast(msg->code); + status_count_++; + } + + void jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr& msg) + { + joint_states_ = *msg; + state_count_++; + } + + void trajectoryCallback(const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr& msg) + { + published_trajectory_ = *msg; + traj_count_++; + } + + void SetUp() override + { + executor_->add_node(servo_test_node_); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + } + + void TearDown() override + { + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + } + + void waitForService() + { + auto logger = servo_test_node_->get_logger(); + while (!switch_input_client_->service_is_ready()) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR(logger, "Interrupted while waiting for the service. Exiting."); + std::exit(EXIT_FAILURE); + } + + rclcpp::sleep_for(std::chrono::milliseconds(500)); + } + RCLCPP_INFO(logger, "SERVICE READY"); + } + + void waitForJointStates() + { + auto logger = servo_test_node_->get_logger(); + auto wait_timeout = rclcpp::Duration::from_seconds(5); + auto start_time = servo_test_node_->now(); + + while (rclcpp::ok() && state_count_ == 0) + { + rclcpp::sleep_for(std::chrono::milliseconds(500)); + auto elapsed_time = servo_test_node_->now() - start_time; + if (elapsed_time >= wait_timeout) + { + RCLCPP_ERROR(logger, "Timed out waiting for joint states"); + FAIL(); + } + } + } + + ServoRosFixture() : state_count_{ 0 }, traj_count_{ 0 } + { + // Create a node to be given to Servo. + servo_test_node_ = std::make_shared("moveit_servo_test"); + executor_ = std::make_shared(); + + status_ = moveit_servo::StatusCode::INVALID; + + status_subscriber_ = servo_test_node_->create_subscription( + "/servo_node/status", rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::ServoStatus::ConstSharedPtr& msg) { return statusCallback(msg); }); + + joint_state_subscriber_ = servo_test_node_->create_subscription( + "/joint_states", rclcpp::SystemDefaultsQoS(), + [this](const sensor_msgs::msg::JointState::ConstSharedPtr& msg) { return jointStateCallback(msg); }); + + trajectory_subscriber_ = servo_test_node_->create_subscription( + "/panda_arm_controller/joint_trajectory", rclcpp::SystemDefaultsQoS(), + [this](const trajectory_msgs::msg::JointTrajectory::ConstSharedPtr& msg) { return trajectoryCallback(msg); }); + + switch_input_client_ = + servo_test_node_->create_client("/servo_node/switch_command_type"); + + waitForService(); + } + + std::shared_ptr servo_test_node_; + + // Executor and a thread to run the executor. + rclcpp::Executor::SharedPtr executor_; + std::thread executor_thread_; + + rclcpp::Subscription::SharedPtr status_subscriber_; + rclcpp::Subscription::SharedPtr joint_state_subscriber_; + rclcpp::Subscription::SharedPtr trajectory_subscriber_; + rclcpp::Client::SharedPtr switch_input_client_; + + sensor_msgs::msg::JointState joint_states_; + trajectory_msgs::msg::JointTrajectory published_trajectory_; + + std::atomic status_count_; + std::atomic status_; + + std::atomic state_count_; + std::atomic traj_count_; +}; diff --git a/moveit_ros/moveit_servo/tests/test_integration.cpp b/moveit_ros/moveit_servo/tests/test_integration.cpp new file mode 100644 index 0000000000..5138fd0757 --- /dev/null +++ b/moveit_ros/moveit_servo/tests/test_integration.cpp @@ -0,0 +1,134 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, 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 : V Mohammed Ibrahim + Desc : Integration tests for the servo c++ API + Title : test_integration.cpp + Project : moveit_servo + Created : 07/07/2023 +*/ + +#include "servo_cpp_fixture.hpp" + +namespace +{ + +TEST_F(ServoCppFixture, JointJogTest) +{ + moveit_servo::StatusCode status_curr, status_next, status_initial; + moveit_servo::JointJogCommand joint_jog_z{ { "panda_joint7" }, { 1.0 } }; + moveit_servo::JointJogCommand zero_joint_jog; + // Compute next state. + servo_test_instance_->setCommandType(moveit_servo::CommandType::JOINT_JOG); + status_initial = servo_test_instance_->getStatus(); + ASSERT_EQ(status_initial, moveit_servo::StatusCode::NO_WARNING); + + moveit_servo::KinematicState curr_state = servo_test_instance_->getNextJointState(zero_joint_jog); + status_curr = servo_test_instance_->getStatus(); + ASSERT_EQ(status_curr, moveit_servo::StatusCode::NO_WARNING); + + moveit_servo::KinematicState next_state = servo_test_instance_->getNextJointState(joint_jog_z); + status_next = servo_test_instance_->getStatus(); + ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING); + + // Check against manually verified value + double delta = next_state.positions[6] - curr_state.positions[6]; + constexpr double tol = 0.00001; + ASSERT_NEAR(delta, 0.02, tol); +} + +TEST_F(ServoCppFixture, TwistTest) +{ + moveit_servo::StatusCode status_curr, status_next, status_initial; + moveit_servo::TwistCommand twist_non_zero{ servo_params_.planning_frame, { 0.0, 0.0, 0.0, 0.0, 0.0, 0.1 } }; + moveit_servo::TwistCommand twist_zero{ servo_params_.planning_frame, { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; + + servo_test_instance_->setCommandType(moveit_servo::CommandType::TWIST); + status_initial = servo_test_instance_->getStatus(); + ASSERT_EQ(status_initial, moveit_servo::StatusCode::NO_WARNING); + moveit_servo::KinematicState curr_state = servo_test_instance_->getNextJointState(twist_zero); + status_curr = servo_test_instance_->getStatus(); + ASSERT_EQ(status_curr, moveit_servo::StatusCode::NO_WARNING); + + moveit_servo::KinematicState next_state = servo_test_instance_->getNextJointState(twist_non_zero); + status_next = servo_test_instance_->getStatus(); + ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING); + + // Check against manually verified value + constexpr double expected_delta = -0.001693; + double delta = next_state.positions[6] - curr_state.positions[6]; + constexpr double tol = 0.00001; + ASSERT_NEAR(delta, expected_delta, tol); +} + +TEST_F(ServoCppFixture, PoseTest) +{ + moveit_servo::StatusCode status_curr, status_next, status_initial; + moveit_servo::PoseCommand zero_pose, non_zero_pose; + zero_pose.frame_id = servo_params_.planning_frame; + zero_pose.pose = servo_test_instance_->getEndEffectorPose(); + + non_zero_pose.frame_id = servo_params_.planning_frame; + non_zero_pose.pose = servo_test_instance_->getEndEffectorPose(); + non_zero_pose.pose.rotate(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ())); + + servo_test_instance_->setCommandType(moveit_servo::CommandType::POSE); + status_initial = servo_test_instance_->getStatus(); + ASSERT_EQ(status_initial, moveit_servo::StatusCode::NO_WARNING); + + moveit_servo::KinematicState curr_state = servo_test_instance_->getNextJointState(zero_pose); + status_curr = servo_test_instance_->getStatus(); + ASSERT_EQ(status_curr, moveit_servo::StatusCode::NO_WARNING); + + moveit_servo::KinematicState next_state = servo_test_instance_->getNextJointState(non_zero_pose); + status_next = servo_test_instance_->getStatus(); + ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING); + + // Check against manually verified value + constexpr double expected_delta = 0.057420; + double delta = next_state.positions[6] - curr_state.positions[6]; + constexpr double tol = 0.00001; + ASSERT_NEAR(delta, expected_delta, tol); +} + +} // namespace + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp new file mode 100644 index 0000000000..ed23f4d69e --- /dev/null +++ b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp @@ -0,0 +1,200 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, 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 : V Mohammed Ibrahim + Desc : ROS API integration tests + Title : test_ros_integration.cpp + Project : moveit_servo + Created : 07/23/2023 +*/ + +#include "servo_ros_fixture.hpp" +#include +#include +#include + +/** + * Order of joints in joint state message + * + * - panda_finger_joint1 + * - panda_joint1 + * - panda_joint2 + * - panda_finger_joint2 + * - panda_joint3 + * - panda_joint4 + * - panda_joint5 + * - panda_joint6 + * - panda_joint7 --> index 8 + */ + +namespace +{ +const int NUM_COMMANDS = 10; +} + +namespace +{ +TEST_F(ServoRosFixture, testJointJog) +{ + waitForJointStates(); + + auto joint_jog_publisher = servo_test_node_->create_publisher( + "/servo_node/delta_joint_cmds", rclcpp::SystemDefaultsQoS()); + + // Call input type service + auto request = std::make_shared(); + request->command_type = 0; + auto response = switch_input_client_->async_send_request(request); + ASSERT_EQ(response.get()->success, true); + + ASSERT_NE(state_count_, 0); + + control_msgs::msg::JointJog jog_cmd; + + jog_cmd.header.frame_id = "panda_link0"; + jog_cmd.joint_names.resize(7); + jog_cmd.velocities.resize(7); + jog_cmd.joint_names = { "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4", + "panda_joint5", "panda_joint6", "panda_joint7" }; + + std::fill(jog_cmd.velocities.begin(), jog_cmd.velocities.end(), 0.0); + + jog_cmd.velocities[6] = 1.0; + + size_t count = 0; + while (rclcpp::ok() && count < NUM_COMMANDS) + { + jog_cmd.header.stamp = servo_test_node_->now(); + joint_jog_publisher->publish(jog_cmd); + count++; + rclcpp::sleep_for(std::chrono::milliseconds(200)); + } + + ASSERT_GE(traj_count_, NUM_COMMANDS); + + moveit_servo::StatusCode status = status_; + RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "Status after jointjog: " << static_cast(status)); + ASSERT_EQ(status_, moveit_servo::StatusCode::NO_WARNING); +} + +TEST_F(ServoRosFixture, testTwist) +{ + waitForJointStates(); + + auto twist_publisher = servo_test_node_->create_publisher( + "/servo_node/delta_twist_cmds", rclcpp::SystemDefaultsQoS()); + + auto request = std::make_shared(); + request->command_type = 1; + auto response = switch_input_client_->async_send_request(request); + ASSERT_EQ(response.get()->success, true); + + ASSERT_NE(state_count_, 0); + + geometry_msgs::msg::TwistStamped twist_cmd; + twist_cmd.header.frame_id = "panda_link0"; // Planning frame + twist_cmd.twist.linear.x = 0.0; + twist_cmd.twist.linear.y = 0.0; + twist_cmd.twist.linear.z = 0.0; + twist_cmd.twist.angular.x = 0.0; + twist_cmd.twist.angular.y = 0.0; + twist_cmd.twist.angular.z = 0.5; + + size_t count = 0; + while (rclcpp::ok() && count < NUM_COMMANDS) + { + twist_cmd.header.stamp = servo_test_node_->now(); + twist_publisher->publish(twist_cmd); + count++; + rclcpp::sleep_for(std::chrono::milliseconds(200)); + } + + ASSERT_GE(traj_count_, NUM_COMMANDS); + + moveit_servo::StatusCode status = status_; + RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "Status after twist: " << static_cast(status)); + ASSERT_EQ(status_, moveit_servo::StatusCode::NO_WARNING); +} + +TEST_F(ServoRosFixture, testPose) +{ + waitForJointStates(); + + auto pose_publisher = servo_test_node_->create_publisher( + "/servo_node/pose_target_cmds", rclcpp::SystemDefaultsQoS()); + + auto request = std::make_shared(); + request->command_type = 2; + auto response = switch_input_client_->async_send_request(request); + ASSERT_EQ(response.get()->success, true); + + geometry_msgs::msg::PoseStamped pose_cmd; + pose_cmd.header.frame_id = "panda_link0"; // Planning frame + + pose_cmd.pose.position.x = 0.3; + pose_cmd.pose.position.y = 0.0; + pose_cmd.pose.position.z = 0.6; + pose_cmd.pose.orientation.x = 0.7; + pose_cmd.pose.orientation.y = -0.7; + pose_cmd.pose.orientation.z = -0.000014; + pose_cmd.pose.orientation.w = -0.0000015; + + ASSERT_NE(state_count_, 0); + + size_t count = 0; + while (rclcpp::ok() && count < NUM_COMMANDS) + { + pose_cmd.header.stamp = servo_test_node_->now(); + pose_publisher->publish(pose_cmd); + count++; + rclcpp::sleep_for(std::chrono::milliseconds(200)); + } + + ASSERT_GE(traj_count_, NUM_COMMANDS); + + moveit_servo::StatusCode status = status_; + RCLCPP_INFO_STREAM(servo_test_node_->get_logger(), "Status after pose: " << static_cast(status)); + ASSERT_EQ(status_, moveit_servo::StatusCode::NO_WARNING); +} + +} // namespace + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/moveit_ros/moveit_servo/tests/test_utils.cpp b/moveit_ros/moveit_servo/tests/test_utils.cpp new file mode 100644 index 0000000000..d54eb08be5 --- /dev/null +++ b/moveit_ros/moveit_servo/tests/test_utils.cpp @@ -0,0 +1,241 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, 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 : V Mohammed Ibrahim + Desc : Tests for utilities that depend on the robot/ robot state. + Title : test_utils.cpp + Project : moveit_servo + Created : 06/20/2023 +*/ + +#include +#include +#include +#include +#include +#include + +namespace +{ + +TEST(ServoUtilsUnitTests, JointLimitVelocityScaling) +{ + using moveit::core::loadTestingRobotModel; + moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); + moveit::core::JointBoundsVector joint_bounds = robot_model->getActiveJointModelsBounds(); + + // Get the upper bound for the velocities of each joint. + Eigen::VectorXd incoming_velocities(joint_bounds.size()); + for (size_t i = 0; i < joint_bounds.size(); i++) + { + const auto joint_bound = (*joint_bounds[i])[0]; + if (joint_bound.velocity_bounded_) + { + incoming_velocities(i) = joint_bound.max_velocity_; + } + } + + // Create incoming velocities with only joint 1 and joint 2 over limit by a factor of 0.1 and 0.05 + // Scale down all other joint velocities by 0.3 to keep it within limits. + incoming_velocities(0) *= 1.1; + incoming_velocities(1) *= 1.05; + incoming_velocities.tail<5>() *= 0.7; + + // The resulting scaling factor selected should be approximately 0.95238 + double user_velocity_override = 0.0; + double scaling_factor = + moveit_servo::jointLimitVelocityScalingFactor(incoming_velocities, joint_bounds, user_velocity_override); + constexpr double tol = 0.001; + ASSERT_NEAR(scaling_factor, 0.95238, tol); +} + +TEST(ServoUtilsUnitTests, validVector) +{ + Eigen::VectorXd valid_vector(7); + valid_vector << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + EXPECT_TRUE(moveit_servo::isValidCommand(valid_vector)); +} + +TEST(ServoUtilsUnitTests, invalidVector) +{ + Eigen::VectorXd invalid_vector(6); + invalid_vector << 0.0, 0.0, 0.0, 0.0, std::nan(""), 0.0; + EXPECT_FALSE(moveit_servo::isValidCommand(invalid_vector)); +} + +TEST(ServoUtilsUnitTests, validTwist) +{ + Eigen::VectorXd valid_vector(6); + valid_vector << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + moveit_servo::TwistCommand valid_twist{ "panda_link0", valid_vector }; + EXPECT_TRUE(moveit_servo::isValidCommand(valid_twist)); +} + +TEST(ServoUtilsUnitTests, emptyTwistFrame) +{ + Eigen::VectorXd invalid_vector(6); + invalid_vector << 0.0, 0.0, 0.0, 0.0, std::nan(""), 0.0; + moveit_servo::TwistCommand invalid_twist; + invalid_twist.velocities = invalid_vector; + EXPECT_FALSE(moveit_servo::isValidCommand(invalid_twist)); +} + +TEST(ServoUtilsUnitTests, invalidTwistVelocities) +{ + Eigen::VectorXd invalid_vector(6); + invalid_vector << 0.0, 0.0, 0.0, 0.0, 0.0, std::nan(""); + moveit_servo::TwistCommand invalid_twist{ "panda_link0", invalid_vector }; + EXPECT_FALSE(moveit_servo::isValidCommand(invalid_twist)); +} + +TEST(ServoUtilsUnitTests, validIsometry) +{ + Eigen::Isometry3d valid_isometry; + valid_isometry.setIdentity(); + EXPECT_TRUE(moveit_servo::isValidCommand(valid_isometry)); +} + +TEST(ServoUtilsUnitTests, invalidIsometry) +{ + Eigen::Isometry3d invalid_isometry; + invalid_isometry.setIdentity(); + invalid_isometry.translation().z() = std::nan(""); + EXPECT_FALSE(moveit_servo::isValidCommand(invalid_isometry)); +} + +TEST(ServoUtilsUnitTests, validPose) +{ + Eigen::Isometry3d valid_isometry; + valid_isometry.setIdentity(); + moveit_servo::PoseCommand valid_pose{ "panda_link0", valid_isometry }; + EXPECT_TRUE(moveit_servo::isValidCommand(valid_pose)); +} + +TEST(ServoUtilsUnitTests, ApproachingSingularityScaling) +{ + using moveit::core::loadTestingRobotModel; + moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); + moveit::core::RobotStatePtr robot_state = std::make_shared(robot_model); + + servo::Params servo_params; + servo_params.move_group_name = "panda_arm"; + const moveit::core::JointModelGroup* joint_model_group = + robot_state->getJointModelGroup(servo_params.move_group_name); + + rclcpp::sleep_for(std::chrono::milliseconds(500)); + Eigen::Vector cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 }; + // Home state + Eigen::Vector state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 }; + robot_state->setJointGroupActivePositions(joint_model_group, state_ready); + auto scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::NO_WARNING); + + // Approach singularity + Eigen::Vector state_approaching_singularity{ 0.0, 0.334, 0.0, -1.177, 0.0, 1.510, 0.785 }; + robot_state->setJointGroupActivePositions(joint_model_group, state_approaching_singularity); + scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY); +} + +TEST(ServoUtilsUnitTests, HaltForSingularityScaling) +{ + using moveit::core::loadTestingRobotModel; + moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); + moveit::core::RobotStatePtr robot_state = std::make_shared(robot_model); + + servo::Params servo_params; + servo_params.move_group_name = "panda_arm"; + const moveit::core::JointModelGroup* joint_model_group = + robot_state->getJointModelGroup(servo_params.move_group_name); + + rclcpp::sleep_for(std::chrono::milliseconds(500)); + Eigen::Vector cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + // Home state + Eigen::Vector state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 }; + robot_state->setJointGroupActivePositions(joint_model_group, state_ready); + auto scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::NO_WARNING); + + // Move to singular state. + Eigen::Vector singular_state{ -0.0001, 0.5690, 0.0005, -0.7782, 0.0, 1.3453, 0.7845 }; + robot_state->setJointGroupActivePositions(joint_model_group, singular_state); + scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::HALT_FOR_SINGULARITY); +} + +TEST(ServoUtilsUnitTests, LeavingSingularityScaling) +{ + using moveit::core::loadTestingRobotModel; + moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); + moveit::core::RobotStatePtr robot_state = std::make_shared(robot_model); + + servo::Params servo_params; + servo_params.move_group_name = "panda_arm"; + const moveit::core::JointModelGroup* joint_model_group = + robot_state->getJointModelGroup(servo_params.move_group_name); + + rclcpp::sleep_for(std::chrono::milliseconds(500)); + Eigen::Vector cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + // Home state + Eigen::Vector state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 }; + robot_state->setJointGroupActivePositions(joint_model_group, state_ready); + auto scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::NO_WARNING); + + // Approach singularity + Eigen::Vector state_approaching_singularity{ 0.0, 0.334, 0.0, -1.177, 0.0, 1.510, 0.785 }; + robot_state->setJointGroupActivePositions(joint_model_group, state_approaching_singularity); + scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY); + + // Move away from singularity + cartesian_delta(0) *= -1; + Eigen::Vector state_leaving_singularity{ 0.0, 0.3458, 0.0, -1.1424, 0.0, 1.4865, 0.785 }; + robot_state->setJointGroupActivePositions(joint_model_group, state_leaving_singularity); + scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); + ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY); +} + +} // namespace + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +}