From 045f6f7fdf0c0e8ed2faa03b4b4ee62aaaee1cda Mon Sep 17 00:00:00 2001 From: ibrahiminfinite Date: Sat, 3 Jun 2023 15:04:08 +0530 Subject: [PATCH] Add C++ APIs: JointJog, Twist and Pose --- moveit_ros/moveit_servo/CMakeLists.txt | 117 +-- .../config/demo_rviz_pose_tracking.rviz | 261 ----- .../panda_simulated_config_pose_tracking.yaml | 60 -- .../config/pose_tracking_settings.yaml | 21 - .../{src => config}/servo_parameters.yaml | 200 ++-- .../demos/cpp_interface/demo_joint_jog.cpp | 105 ++ .../demos/cpp_interface/demo_pose.cpp | 149 +++ .../demos/cpp_interface/demo_twist.cpp | 107 ++ ...ollision_check.h => collision_monitor.hpp} | 17 +- .../moveit_servo/command_processor.hpp | 133 +++ ...{make_shared_from_pool.h => datatypes.hpp} | 61 +- .../include/moveit_servo/pose_tracking.h | 195 ---- .../moveit_servo/include/moveit_servo/servo.h | 105 -- .../include/moveit_servo/servo.hpp | 186 ++++ .../include/moveit_servo/servo_calcs.h | 271 ----- .../include/moveit_servo/servo_node.h | 83 -- .../include/moveit_servo/servo_server.h | 51 - .../{status_codes.h => status_codes.hpp} | 18 +- .../include/moveit_servo/utilities.h | 136 --- .../include/moveit_servo/utils.hpp | 140 +++ ...ple.launch.py => demo_joint_jog.launch.py} | 36 +- .../demo_pose.launch.py} | 88 +- .../demo_twist.launch.py} | 111 +- .../launch/pose_tracking_example.launch.py | 110 -- ...lision_check.cpp => collision_monitor.cpp} | 20 +- .../moveit_servo/src/command_processor.cpp | 275 +++++ .../cpp_interface_demo/pose_tracking_demo.cpp | 185 ---- moveit_ros/moveit_servo/src/pose_tracking.cpp | 405 -------- moveit_ros/moveit_servo/src/servo.cpp | 426 +++++++- moveit_ros/moveit_servo/src/servo_calcs.cpp | 966 ------------------ moveit_ros/moveit_servo/src/servo_node.cpp | 163 --- .../moveit_servo/src/servo_node_main.cpp | 53 - .../teleop_demo/joystick_servo_example.cpp | 279 ----- moveit_ros/moveit_servo/src/utilities.cpp | 305 ------ moveit_ros/moveit_servo/src/utils.cpp | 308 ++++++ .../config/collision_start_positions.yaml | 16 - .../test/config/pose_tracking_settings.yaml | 22 - .../test/config/servo_settings.yaml | 58 -- .../test/launch/test_servo_collision.test.py | 27 - .../launch/test_servo_integration.test.py | 24 - .../moveit_servo/test/pose_tracking_test.cpp | 220 ---- .../test/publish_fake_jog_commands.cpp | 99 -- .../test/servo_calcs_unit_tests.cpp | 176 ---- .../test/servo_launch_test_common.hpp | 426 -------- .../test/test_servo_collision.cpp | 155 --- .../test/test_servo_interface.cpp | 197 ---- 46 files changed, 2068 insertions(+), 5498 deletions(-) delete mode 100644 moveit_ros/moveit_servo/config/demo_rviz_pose_tracking.rviz delete mode 100644 moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml delete mode 100644 moveit_ros/moveit_servo/config/pose_tracking_settings.yaml rename moveit_ros/moveit_servo/{src => config}/servo_parameters.yaml (73%) create mode 100644 moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp create mode 100644 moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp create mode 100644 moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp rename moveit_ros/moveit_servo/include/moveit_servo/{collision_check.h => collision_monitor.hpp} (92%) create mode 100644 moveit_ros/moveit_servo/include/moveit_servo/command_processor.hpp rename moveit_ros/moveit_servo/include/moveit_servo/{make_shared_from_pool.h => datatypes.hpp} (66%) delete mode 100644 moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h delete mode 100644 moveit_ros/moveit_servo/include/moveit_servo/servo.h create mode 100644 moveit_ros/moveit_servo/include/moveit_servo/servo.hpp delete mode 100644 moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h delete mode 100644 moveit_ros/moveit_servo/include/moveit_servo/servo_node.h delete mode 100644 moveit_ros/moveit_servo/include/moveit_servo/servo_server.h rename moveit_ros/moveit_servo/include/moveit_servo/{status_codes.h => status_codes.hpp} (90%) delete mode 100644 moveit_ros/moveit_servo/include/moveit_servo/utilities.h create mode 100644 moveit_ros/moveit_servo/include/moveit_servo/utils.hpp rename moveit_ros/moveit_servo/launch/{servo_example.launch.py => demo_joint_jog.launch.py} (72%) rename moveit_ros/moveit_servo/{test/launch/test_servo_pose_tracking.test.py => launch/demo_pose.launch.py} (56%) rename moveit_ros/moveit_servo/{test/launch/servo_launch_test_common.py => launch/demo_twist.launch.py} (50%) delete mode 100644 moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py rename moveit_ros/moveit_servo/src/{collision_check.cpp => collision_monitor.cpp} (92%) create mode 100644 moveit_ros/moveit_servo/src/command_processor.cpp delete mode 100644 moveit_ros/moveit_servo/src/cpp_interface_demo/pose_tracking_demo.cpp delete mode 100644 moveit_ros/moveit_servo/src/pose_tracking.cpp delete mode 100644 moveit_ros/moveit_servo/src/servo_calcs.cpp delete mode 100644 moveit_ros/moveit_servo/src/servo_node.cpp delete mode 100644 moveit_ros/moveit_servo/src/servo_node_main.cpp delete mode 100644 moveit_ros/moveit_servo/src/teleop_demo/joystick_servo_example.cpp delete mode 100644 moveit_ros/moveit_servo/src/utilities.cpp create mode 100644 moveit_ros/moveit_servo/src/utils.cpp delete mode 100644 moveit_ros/moveit_servo/test/config/collision_start_positions.yaml delete mode 100644 moveit_ros/moveit_servo/test/config/pose_tracking_settings.yaml delete mode 100644 moveit_ros/moveit_servo/test/config/servo_settings.yaml delete mode 100644 moveit_ros/moveit_servo/test/launch/test_servo_collision.test.py delete mode 100644 moveit_ros/moveit_servo/test/launch/test_servo_integration.test.py delete mode 100644 moveit_ros/moveit_servo/test/pose_tracking_test.cpp delete mode 100644 moveit_ros/moveit_servo/test/publish_fake_jog_commands.cpp delete mode 100644 moveit_ros/moveit_servo/test/servo_calcs_unit_tests.cpp delete mode 100644 moveit_ros/moveit_servo/test/servo_launch_test_common.hpp delete mode 100644 moveit_ros/moveit_servo/test/test_servo_collision.cpp delete mode 100644 moveit_ros/moveit_servo/test/test_servo_interface.cpp diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index dc9fccbf969..2f10e60644a 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -42,60 +42,39 @@ 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 + src/collision_monitor.cpp + src/command_processor.cpp src/servo.cpp - src/servo_calcs.cpp - src/utilities.cpp + src/utils.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) -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) - -##################### -## Component Nodes ## -##################### - -# 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") - -# 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") - ###################### ## 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}) +# An 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) +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}) +# An executable node for the twist demo +add_executable(demo_twist demos/cpp_interface/demo_twist.cpp ) +target_link_libraries(demo_twist moveit_servo_lib) +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 -) +# An executable node for the pose demo +add_executable(demo_pose demos/cpp_interface/demo_pose.cpp ) +target_link_libraries(demo_pose moveit_servo_lib) +ament_target_dependencies(demo_pose ${THIS_PACKAGE_INCLUDE_DEPENDS}) ############# ## Install ## @@ -106,9 +85,6 @@ install( TARGETS moveit_servo_lib moveit_servo_lib_parameters - pose_tracking - servo_node - servo_controller_input EXPORT moveit_servoTargets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib @@ -119,9 +95,9 @@ install( # Install Binaries install( TARGETS - servo_node_main - servo_pose_tracking_demo - fake_command_publisher + demo_joint_jog + demo_twist + demo_pose ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/moveit_servo @@ -135,57 +111,4 @@ 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) - -endif() - ament_package() diff --git a/moveit_ros/moveit_servo/config/demo_rviz_pose_tracking.rviz b/moveit_ros/moveit_servo/config/demo_rviz_pose_tracking.rviz deleted file mode 100644 index d93be94cdef..00000000000 --- a/moveit_ros/moveit_servo/config/demo_rviz_pose_tracking.rviz +++ /dev/null @@ -1,261 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - Splitter Ratio: 0.5 - Tree Height: 628 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: moveit_rviz_plugin/PlanningScene - Enabled: true - Move Group Namespace: "" - Name: PlanningScene - Planning Scene Topic: /monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 0.8999999761581421 - Scene Color: 50; 230; 50 - Scene Display Time: 0.20000000298023224 - Show Scene Geometry: true - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels - Scene Robot: - Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - panda_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_leftfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link8: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_rightfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Robot Alpha: 1 - Show Robot Collision: false - Show Robot Visual: true - Value: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - panda_hand: - Value: true - panda_leftfinger: - Value: false - panda_link0: - Value: false - panda_link1: - Value: false - panda_link2: - Value: false - panda_link3: - Value: false - panda_link4: - Value: false - panda_link5: - Value: false - panda_link6: - Value: false - panda_link7: - Value: false - panda_link8: - Value: false - panda_rightfinger: - Value: false - world: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - world: - panda_link0: - panda_link1: - panda_link2: - panda_link3: - panda_link4: - panda_link5: - panda_link6: - panda_link7: - panda_link8: - panda_hand: - panda_leftfinger: - {} - panda_rightfinger: - {} - Update Interval: 0 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 2.155569553375244 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -0.08608254045248032 - Y: -0.20677587389945984 - Z: 0.3424459993839264 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.4603978991508484 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.8753982782363892 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 857 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000216000002fffc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ff000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f000002d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000416000002ff00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1586 - X: 1179 - Y: 393 diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml deleted file mode 100644 index d0ee8d6b408..00000000000 --- a/moveit_ros/moveit_servo/config/panda_simulated_config_pose_tracking.yaml +++ /dev/null @@ -1,60 +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.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] - -# 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 - -# What to publish? Can save some bandwidth as most robots only require positions or velocities -publish_joint_positions: true -publish_joint_velocities: true -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 '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 - -## Configure handling of singularities and joint limits -lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) -hard_stop_singularity_threshold: 30.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. -leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) - -## Topic names -cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands -joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands -joint_topic: /joint_states -status_topic: ~/status # Publish status to this topic -command_out_topic: /panda_arm_controller/joint_trajectory # Publish outgoing commands here - -## Collision checking for the entire robot body -check_collisions: true # Check collisions? -collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. -self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] -scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] 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 e4b803774d1..00000000000 --- 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 73% rename from moveit_ros/moveit_servo/src/servo_parameters.yaml rename to moveit_ros/moveit_servo/config/servo_parameters.yaml index 774a085991b..9a48aa46b33 100644 --- a/moveit_ros/moveit_servo/src/servo_parameters.yaml +++ b/moveit_ros/moveit_servo/config/servo_parameters.yaml @@ -89,29 +89,30 @@ servo: ################################ GENERAL CONFIG ############################# enable_parameter_update: { type: bool, - default_value: false, + default_value: true, 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" } - scale: - linear: { - type: double, - default_value: 0.4, - description: "Max linear velocity. Unit is [m/s]. Only used for Cartesian commands." - } - rotational: { - type: double, - default_value: 0.8, - description: "Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands." - } - joint: { - type: double, - default_value: 0.5, - description: "Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic." - } + linear_scale: { + type: double, + default_value: 0.4, + description: "Max linear velocity. Unit is [m/s]. Only used for Cartesian commands." + } + + rotational_scale: { + type: double, + default_value: 0.8, + description: "Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands" + } + + joint_scale: { + type: double, + default_value: 0.5, + description: "Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic" + } override_velocity_scaling_factor: { type: double, @@ -263,80 +264,91 @@ 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" - } - - 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" - } - - angular_derivative_gain: { - type: double, - default_value: 0.0, - description: "Derivative gain value for angular control" - } + pose_tracking: + x_pid: + kp: { + type: double, + default_value: 1.5, + description: "Proportional gain value for the controller in x direction" + } + + ki: { + type: double, + default_value: 0.0, + description: "Integral gain value for the controller in x direction" + } + + kd: { + type: double, + default_value: 0.0, + description: "Derivative gain value for the controller in x direction" + } + + y_pid: + kp: { + type: double, + default_value: 1.5, + description: "Proportional gain value for the controller in y direction" + } + + ki: { + type: double, + default_value: 0.0, + description: "Integral gain value for the controller in y direction" + } + + kd: { + type: double, + default_value: 0.0, + description: "Derivative gain value for the controller in y direction" + } + + z_pid: + kp: { + type: double, + default_value: 1.5, + description: "Proportional gain value for the controller in z direction" + } + + ki: { + type: double, + default_value: 0.0, + description: "Integral gain value for the controller in z direction" + } + + kd: { + type: double, + default_value: 0.0, + description: "Derivative gain value for the controller in z direction" + } + + q_pid: + kp: { + type: double, + default_value: 1.5, + description: "Proportional gain value for angular control" + } + + ki: { + type: double, + default_value: 0.0, + description: "Integral gain value for angular control" + } + + kd: { + type: double, + default_value: 0.0, + description: "Derivative gain value for angular control" + } + + windup_limit: { + type: double, + default_value: 0.05, + description: "Maximum value of error integral for all PID controllers" + } + + use_anti_windup: { + type: bool, + default_value: true, + description: "If the controller should use anit windup" + } 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 00000000000..f9816bd73a4 --- /dev/null +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp @@ -0,0 +1,105 @@ +/******************************************************************************* + * 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 : joint_jog_demo.cpp + * Project : moveit_servo + * Created : 27/05/2023 + * Author : V Mohammed Ibrahim + * + * Description : Example of controlling a robot through joint jog commands via the C++ API. + */ + +#include +#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. + auto servo_node = std::make_shared("servo_node"); + + // Get the servo parameters. + std::string param_namespace = "moveit_servo"; + auto servo_param_listener = std::make_shared(servo_node, param_namespace); + auto servo_params = servo_param_listener->get_params(); + + // The publisher to send trajectory message to the robot controller. + rclcpp::Publisher::SharedPtr trajectory_outgoing_cmd_pub = + servo_node->create_publisher(servo_params.command_out_topic, + rclcpp::SystemDefaultsQoS()); + + // Create the servo object + auto servo = Servo(servo_node, servo_param_listener); + + // Wait for some time, so that we can actually see when the robot moves. + // This is just for convenience, should not be used for sync in real application. + std::this_thread::sleep_for(std::chrono::seconds(3)); + + // Frquency at which the commands will be send to robot controller. + rclcpp::WallRate rate(1.0 / servo_params.publish_period); + + // Set the command type for servo. + servo.incomingCommandType(CommandType::JOINT_JOG); + + RCLCPP_INFO_STREAM(LOGGER, servo.getStatusMessage()); + while (rclcpp::ok() && servo.getStatus() == StatusCode::NO_WARNING) + { + // Move only the 7th joint + moveit_servo::JointJog vec(7); + vec << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; // rad/s + + auto joint_state = servo.getNextJointState(vec); + + // Send the command to robot controller only if the command was valid. + if (servo.getStatus() != StatusCode::INVALID) + { + auto joint_trajectory = composeTrajectoryMessage(servo_params, joint_state); + trajectory_outgoing_cmd_pub->publish(joint_trajectory); + } + + rate.sleep(); + } + RCLCPP_INFO_STREAM(LOGGER, servo.getStatusMessage()); + 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 00000000000..7ac668ee31b --- /dev/null +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp @@ -0,0 +1,149 @@ +/******************************************************************************* + * 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 : demo_pose.cpp + * Project : moveit_servo + * Created : 07/06/2023 + * Author : V Mohammed Ibrahim + * Description : Example of controlling a robot through pose commands via the C++ API. + */ + +#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. + auto servo_node = std::make_shared("servo_node"); + + // Get the servo parameters. + std::string param_namespace = "moveit_servo"; + auto servo_param_listener = std::make_shared(servo_node, param_namespace); + auto servo_params = servo_param_listener->get_params(); + + // The publisher to send trajectory message to the robot controller. + rclcpp::Publisher::SharedPtr trajectory_outgoing_cmd_pub = + servo_node->create_publisher(servo_params.command_out_topic, + rclcpp::SystemDefaultsQoS()); + + // Create the servo object + auto servo = Servo(servo_node, servo_param_listener); + + // Wait for some time, so that we can actually see when the robot moves. + // This is just for convenience, should not be used for sync in real application. + std::this_thread::sleep_for(std::chrono::seconds(3)); + + // Frquency at which the commands will be send to robot controller. + rclcpp::WallRate rate(1.0 / servo_params.publish_period); + + // Set the command type for servo. + servo.incomingCommandType(CommandType::POSE); + + // 1. The workflow for when command is not in planning frame. + Pose target_pose_ee_frame; + target_pose_ee_frame.frame_id = servo_params.ee_frame_name; + target_pose_ee_frame.pose.setIdentity(); + // Set the target pose to +10 cm in the z direction in ee frame. + target_pose_ee_frame.pose.translate(Eigen::Vector3d(0.0, 0.0, 0.1)); + // Set rotation to +45 degrees from current angle about z + target_pose_ee_frame.pose.rotate(Eigen::Quaterniond(servo.getEndEffectorPose().rotation())); + target_pose_ee_frame.pose.rotate(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); + // Servo only accepts commands in planning_frame, so use convert it before sending. + target_pose_ee_frame = servo.toPlanningFrame(target_pose_ee_frame); + + // 2. The workflow for when command is in planning frame. + Pose target_pose_planning_frame; + target_pose_planning_frame.frame_id = servo_params.planning_frame; + // Set position to 10 cm in +z direction from current ee frame position in planning frame. + target_pose_planning_frame.pose = servo.getEndEffectorPose(); + target_pose_planning_frame.pose.translate(Eigen::Vector3d(0.0, 0.0, -0.1)); + // Set angle to 45 degree from current angle about z, in planning frame. + target_pose_planning_frame.pose.rotate(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ())); + + // Set tolerances for the target. + const double linear_tolerance{ 0.001 }, angular_tolerance{ 0.01 }; + + auto move_to_pose = [&](const auto& target_pose) { + bool reached = false; + while (rclcpp::ok() && servo.getStatus() == StatusCode::NO_WARNING) + { + const bool satisfies_angular_tolerance = + servo.getEndEffectorPose().rotation().isApprox(target_pose.pose.rotation(), angular_tolerance); + const bool satisfies_linear_tolerance = + servo.getEndEffectorPose().translation().isApprox(target_pose.pose.translation(), linear_tolerance); + reached = satisfies_angular_tolerance && satisfies_linear_tolerance; + if (reached) + { + break; + } + else + { + auto joint_state = servo.getNextJointState(target_pose); + // Send the command to robot controller only if the command was valid. + if (servo.getStatus() != StatusCode::INVALID) + { + auto joint_trajectory = composeTrajectoryMessage(servo_params, joint_state); + trajectory_outgoing_cmd_pub->publish(joint_trajectory); + } + } + rate.sleep(); + } + servo.resetPoseControllers(); + return reached; + }; + + // Move down and then up. + RCLCPP_INFO_STREAM(LOGGER, servo.getStatusMessage()); + if (move_to_pose(target_pose_ee_frame)) + { + RCLCPP_INFO_STREAM(LOGGER, "End-Effector frame target Pose achieved"); + } + if (move_to_pose(target_pose_planning_frame)) + { + RCLCPP_INFO_STREAM(LOGGER, "Planning frame target pose achieved"); + } + RCLCPP_INFO_STREAM(LOGGER, servo.getStatusMessage()); + 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 00000000000..38e920a34e0 --- /dev/null +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp @@ -0,0 +1,107 @@ +/******************************************************************************* + * 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 : twist_demo.cpp + * Project : moveit_servo + * Created : 01/06/2023 + * Author : V Mohammed Ibrahim + * + * Description : Example of controlling a robot through twist commands via the C++ API. + */ + +#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. + auto servo_node = std::make_shared("servo_node"); + + // Get the servo parameters. + std::string param_namespace = "moveit_servo"; + auto servo_param_listener = std::make_shared(servo_node, param_namespace); + auto servo_params = servo_param_listener->get_params(); + + // The publisher to send trajectory message to the robot controller. + rclcpp::Publisher::SharedPtr trajectory_outgoing_cmd_pub = + servo_node->create_publisher(servo_params.command_out_topic, + rclcpp::SystemDefaultsQoS()); + + // Create the servo object + auto servo = Servo(servo_node, servo_param_listener); + + // Wait for some time, so that we can actually see when the robot moves. + // This is just for convenience, should not be used for sync in real application. + std::this_thread::sleep_for(std::chrono::seconds(3)); + + // Frquency at which the commands will be send to robot controller. + rclcpp::WallRate rate(1.0 / servo_params.publish_period); + + // Set the command type for servo. + servo.incomingCommandType(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 + Twist twist{ servo_params.ee_frame_name, { 0.0, 0.0, 0.1, 0.0, 0.0, 0.5 } }; + // Servo expects the command to be in planning frame, use the helper method to convert it. + twist = servo.toPlanningFrame(twist); + + RCLCPP_INFO_STREAM(LOGGER, "SERVO STATUS: " << servo.getStatusMessage()); + while (rclcpp::ok() && servo.getStatus() == StatusCode::NO_WARNING) + { + auto joint_state = servo.getNextJointState(twist); + + // Send the command to robot controller only if the command was valid. + if (servo.getStatus() != StatusCode::INVALID) + { + auto joint_trajectory = composeTrajectoryMessage(servo_params, joint_state); + trajectory_outgoing_cmd_pub->publish(joint_trajectory); + } + + rate.sleep(); + } + RCLCPP_INFO_STREAM(LOGGER, "SERVO STATUS: " << servo.getStatusMessage()); + rclcpp::shutdown(); +} diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp similarity index 92% rename from moveit_ros/moveit_servo/include/moveit_servo/collision_check.h rename to moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp index 22f3db24f7f..c19151f9e5f 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp @@ -1,12 +1,7 @@ /******************************************************************************* - * 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 + * Copyright (c) 2021, PickNik Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -35,6 +30,14 @@ * 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 : 1/11/2019 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + * + * Description: Monitors the planning scene for collision and publishes the velocity scaling. + */ #pragma once @@ -92,7 +95,7 @@ class CollisionCheck planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; // Robot state and collision matrix from planning scene - std::shared_ptr current_state_; + std::shared_ptr robot_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. diff --git a/moveit_ros/moveit_servo/include/moveit_servo/command_processor.hpp b/moveit_ros/moveit_servo/include/moveit_servo/command_processor.hpp new file mode 100644 index 00000000000..70717a59d4b --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/command_processor.hpp @@ -0,0 +1,133 @@ +/******************************************************************************* + * 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_processor.hpp + * Project : moveit_servo + * Created : 04/06/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 + +#include +#include +#include + +namespace moveit_servo +{ + +class CommandProcessor +{ +public: + CommandProcessor(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const moveit::core::JointModelGroup* joint_model_group, moveit::core::RobotStatePtr& current_state, + servo::Params& servo_params, StatusCode& servo_status); + + /** + * \brief Compute the change in joint position for the given joint jog command. + * @param command The joint jog command. + * @return The joint position change required (delta). + */ + Eigen::VectorXd jointDeltaFromCommand(const JointJog& command); + + /** + * \brief Compute the change in joint position for the given twist command. + * @param command The twist command. + * @return The joint position change required (delta). + */ + Eigen::VectorXd jointDeltaFromCommand(const Twist& command); + + /** + * \brief Compute the change in joint position for the given pose command. + * @param command The pose command. + * @return The joint position change required (delta). + */ + Eigen::VectorXd jointDeltaFromCommand(const Pose& command); + + /** + * \brief Returns the end effector pose in planning frame + */ + const Eigen::Isometry3d getEndEffectorPose(); + + void resetControllers(); + +private: + /** + * \brief Set the IK solver that servo will use. If the robot does not have one, inverse jacobian will be used instead. + */ + void setIKSolver(); + + /** + * \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 Computes the required change in joint angles for given cartesian change, using the robots IK solver. + * @param carteisan_position_delta The change in cartesian position. + * @return The required joing angle deltas. + */ + Eigen::VectorXd deltaFromIkSolver(const Eigen::VectorXd& cartesian_position_delta); + + // Variables + size_t num_joints_; + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor_; + const moveit::core::JointModelGroup* joint_model_group_; + moveit::core::RobotStatePtr& robot_state_; + servo::Params& servo_params_; + StatusCode& servo_status_; + + kinematics::KinematicsBaseConstPtr ik_solver_ = nullptr; + + uint64_t controller_period_; + std::map controllers_; +}; + +} // 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/datatypes.hpp similarity index 66% rename from moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h rename to moveit_ros/moveit_servo/include/moveit_servo/datatypes.hpp index fa0436a3a67..a5d7e924700 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/datatypes.hpp @@ -1,9 +1,4 @@ /******************************************************************************* - * 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 @@ -36,22 +31,54 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ +/* Title : datatypes.hpp + * Project : moveit_servo + * Created : 05/06/2023 + * Author : V Mohammed Ibrahim + * + * Description : The custom datatypes used by Moveit Servo. + */ + #pragma once -#include -#include +namespace moveit_servo +{ +// Datatypes used by servo + +typedef Eigen::VectorXd JointJog; -namespace moveit +struct Pose { -namespace util + std::string frame_id; + Eigen::Isometry3d pose; +}; + +struct Twist { -// Useful template for creating messages from a message pool -template -std::shared_ptr make_shared_from_pool() + std::string frame_id; + Eigen::Vector velocities; +}; + +typedef std::variant ServoInput; + +struct KinematicState { - using allocator_t = boost::fast_pool_allocator>; - return std::allocate_shared(allocator_t()); -} + std::vector joint_names; + std::vector positions, velocities, accelerations; -} // namespace util -} // namespace moveit + KinematicState(const int num_joints) + { + joint_names.resize(num_joints); + positions.resize(num_joints); + velocities.resize(num_joints); + accelerations.resize(num_joints); + } +}; + +enum class CommandType +{ + JOINT_JOG = 0, + TWIST = 1, + POSE = 2 +}; +} // namespace moveit_servo 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 c99d239a61f..00000000000 --- 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.h b/moveit_ros/moveit_servo/include/moveit_servo/servo.h deleted file mode 100644 index a18909dea2b..00000000000 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.h +++ /dev/null @@ -1,105 +0,0 @@ -/******************************************************************************* - * 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 - * 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 - -// System -#include - -// Moveit2 -#include -#include -#include - -namespace moveit_servo -{ -/** - * Class Servo - Jacobian based robot control with collision avoidance. - */ -class Servo -{ -public: - Servo(const rclcpp::Node::SharedPtr& node, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::shared_ptr& servo_param_listener); - - /** \brief Start servo node */ - void start(); - - /** \brief Stop servo node */ - void stop(); - - /** - * 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); - - // Give test access to private/protected methods - friend class ServoFixture; - -private: - // 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_; - - ServoCalcs servo_calcs_; - CollisionCheck collision_checker_; -}; - -// ServoPtr using alias -using ServoPtr = 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 00000000000..5cedfaace1c --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -0,0 +1,186 @@ +/******************************************************************************* + * 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 : 17/05/2023 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, V Mohammed Ibrahim + * + * Description : The core servoing logic. + */ + +#pragma once + +// Standard Library +#include + +// ROS +#include +#include +#include +#include + +// MoveIt +#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); + + /** + * \brief Computes the required joint state 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 Compute the change in joint position for the received command. + * @param command The incoming servo command. + * @return The joint position change required (delta). + */ + Eigen::VectorXd jointDeltaFromCommand(const ServoInput& command); + + /** + * \brief Set the type of incoming servo command. + * @param command_type The type of command servo should expect. + */ + void incomingCommandType(const CommandType& command_type); + + /** + * \brief Get the type of command that servo is currently expecting. + * @return The type of command. + */ + CommandType incomingCommandType(); + + /** + * \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(); + + StatusCode moveToPose(const Pose& target_pose); + + void resetPoseControllers(); + + Pose toPlanningFrame(const Pose& command); + + Twist toPlanningFrame(const Twist& command); + +private: + /** + * \brief Validate the servo parameters + * @param servo_params The servo parameters + */ + void validateParams(const servo::Params& servo_params); + + /** + * \brief Creates the planning scene monitor used by servo + */ + void createPlanningSceneMonitor(); + + /** + * \brief create and initialize the smoothing plugin to be used by servo. + */ + void setSmoothingPlugin(); + + /** + * \brief The callback for velocity scaling values from collision checker. + */ + void collisionVelocityScaleCB(const std_msgs::msg::Float64::ConstSharedPtr& msg); + + /** + * \brief Updates the servo parameters and performs some validations. + */ + void updateParams(); + + /** + * \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 + + const rclcpp::Node::SharedPtr node_; + std::unique_ptr command_processor_; + // This needs to be threadsafe so it can be updated in realtime with Dynamic Reconfigure + std::atomic incoming_command_type_; + + servo::Params servo_params_; + std::shared_ptr servo_param_listener_; + + moveit::core::RobotStatePtr robot_state_; + const moveit::core::JointModelGroup* joint_model_group_; + + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + std::atomic collision_velocity_scale_ = 1.0; + std::unique_ptr collision_checker_; + rclcpp::Subscription::SharedPtr collision_velocity_scale_sub_; + + pluginlib::UniquePtr smoother_; + + size_t num_joints_; + std::vector joint_names_; + moveit::core::JointBoundsVector joint_bounds_; + + StatusCode servo_status_; +}; + +} // 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 39f780e2218..00000000000 --- 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 762088b55e8..00000000000 --- 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_server.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_server.h deleted file mode 100644 index f16e1320399..00000000000 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_server.h +++ /dev/null @@ -1,51 +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_server.h - * Project : moveit_servo - * Created : 07/13/2020 - * Author : Adam Pettinger - */ - -#pragma once - -#pragma message("Header `servo_server.h` is deprecated, please use `servo_node.h`") - -#include - -namespace moveit_servo -{ -using ServoServer [[deprecated("use ServoNode from servo_node.h")]] = ServoNode; - -} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h b/moveit_ros/moveit_servo/include/moveit_servo/status_codes.hpp similarity index 90% rename from moveit_ros/moveit_servo/include/moveit_servo/status_codes.h rename to moveit_ros/moveit_servo/include/moveit_servo/status_codes.hpp index f5942fa4bed..79c6bd7440b 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/status_codes.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 @@ -35,7 +30,14 @@ * 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 : status_codes.h + * Project : moveit_servo + * Created : 2/25/2019 + * Author : Andy Zelenak + * + * Description : The status codes used by MoveIt Servo to indicate the various conditions. + */ #pragma once #include @@ -53,7 +55,6 @@ enum class StatusCode : int8_t HALT_FOR_COLLISION = 4, JOINT_BOUND = 5, DECELERATE_FOR_LEAVING_SINGULARITY = 6, - PAUSED = 7 }; const std::unordered_map SERVO_STATUS_CODE_MAP( @@ -64,6 +65,5 @@ const std::unordered_map SERVO_STATUS_CODE_MAP( { 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::DECELERATE_FOR_LEAVING_SINGULARITY, "Moving away from a singularity, decelerating" } }); } // 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 30bedbd0617..00000000000 --- 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.hpp b/moveit_ros/moveit_servo/include/moveit_servo/utils.hpp new file mode 100644 index 00000000000..74130284984 --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/utils.hpp @@ -0,0 +1,140 @@ +/******************************************************************************* + * 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 : 17/05/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 + +namespace moveit_servo +{ + +/** + * \brief Checks if a transform exists to the given frame. + * @param current_state The current robot state. + * @param frame_name The name of the frame for which we want to know if transform exists. + * @return True if transform exists, else false + */ +bool transformExists(const moveit::core::RobotStatePtr& current_state, const std::string& frame_name); + +/** + * \brief Checks if a given command is valid. + * @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 command is valid. + * @param command The command to be checked. + * @return True if the command is valid, else False. + */ +bool isValidCommand(const Eigen::Isometry3d& 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 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::JointModelGroup* joint_model_group, + const moveit::core::RobotStatePtr& current_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 velocityScalingFactor(const Eigen::VectorXd& velocities, const moveit::core::JointBoundsVector& joint_bounds, + double scaling_override); + +/** + * \brief Finds the joints that are going past allowable joint limits. + * @param positions The joints positions. + * @param velocities The current commanded velocities. + * @param joint_bounds The bounding information for the robot joints. + * @param margin The buffer before the actual limit. + * @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 Creates the PID controllers used for pose tracking based on config provided in servo parameters. + * The parameters can be updated dynamically. + * @param servo_params The servo parameters, used to get pid information. + */ +std::map createControllers(const servo::Params& servo_params); +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/launch/servo_example.launch.py b/moveit_ros/moveit_servo/launch/demo_joint_jog.launch.py similarity index 72% rename from moveit_ros/moveit_servo/launch/servo_example.launch.py rename to moveit_ros/moveit_servo/launch/demo_joint_jog.launch.py index 224cb6bf552..6167c230833 100644 --- a/moveit_ros/moveit_servo/launch/servo_example.launch.py +++ b/moveit_ros/moveit_servo/launch/demo_joint_jog.launch.py @@ -2,18 +2,11 @@ import launch import launch_ros from ament_index_python.packages import get_package_share_directory -from launch.conditions import IfCondition, UnlessCondition -from launch.substitutions import LaunchConfiguration from launch_param_builder import ParameterBuilder from moveit_configs_utils import MoveItConfigsBuilder 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") @@ -84,19 +77,6 @@ def generate_launch_description(): 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, - moveit_config.robot_description, - moveit_config.robot_description_semantic, - ], - condition=UnlessCondition(launch_as_standalone_node), - ), launch_ros.descriptions.ComposableNode( package="robot_state_publisher", plugin="robot_state_publisher::RobotStatePublisher", @@ -109,25 +89,14 @@ 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="demo_joint_jog", parameters=[ servo_params, low_pass_filter_coeff, @@ -136,7 +105,6 @@ def generate_launch_description(): moveit_config.robot_description_kinematics, ], output="screen", - condition=IfCondition(launch_as_standalone_node), ) return launch.LaunchDescription( diff --git a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py b/moveit_ros/moveit_servo/launch/demo_pose.launch.py similarity index 56% rename from moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py rename to moveit_ros/moveit_servo/launch/demo_pose.launch.py index f96d7523a13..674c40b9da1 100644 --- a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py +++ b/moveit_ros/moveit_servo/launch/demo_pose.launch.py @@ -1,31 +1,44 @@ import os -import pytest import launch 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 -def generate_servo_test_description( - *args, gtest_name: launch.some_substitutions_type.SomeSubstitutionsType -): +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 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") + .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"), @@ -57,9 +70,9 @@ def generate_servo_test_description( arguments=["panda_arm_controller", "-c", "/controller_manager"], ) - # Component nodes for tf and Servo - test_container = launch_ros.actions.ComposableNodeContainer( - name="test_pose_tracking_container", + # 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", @@ -74,56 +87,33 @@ def generate_servo_test_description( package="tf2_ros", plugin="tf2_ros::StaticTransformBroadcasterNode", name="static_tf2_broadcaster", - parameters=[{"/child_frame_id": "panda_link0", "/frame_id": "world"}], + parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}], ), ], output="screen", ) - - pose_tracking_gtest = launch_ros.actions.Node( - executable=launch.substitutions.PathJoinSubstitution( - [launch.substitutions.LaunchConfiguration("test_binary_dir"), gtest_name] - ), + # 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=[ - 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", ) return launch.LaunchDescription( [ - launch.actions.DeclareLaunchArgument( - name="test_binary_dir", - description="Binary directory of package " - "containing test executables", - ), + rviz_node, ros2_control_node, joint_state_broadcaster_spawner, panda_arm_controller_spawner, - test_container, - launch.actions.TimerAction(period=2.0, actions=[pose_tracking_gtest]), - launch_testing.actions.ReadyToTest(), + servo_node, + container, ] - ), { - "test_container": test_container, - "servo_gtest": pose_tracking_gtest, - "ros2_control_node": ros2_control_node, - } - - -def generate_test_description(): - return generate_servo_test_description(gtest_name="test_servo_pose_tracking") - - -class TestGTestProcessActive(unittest.TestCase): - 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 - ): - launch_testing.asserts.assertExitCodes(proc_info, process=servo_gtest) + ) diff --git a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py b/moveit_ros/moveit_servo/launch/demo_twist.launch.py similarity index 50% rename from moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py rename to moveit_ros/moveit_servo/launch/demo_twist.launch.py index 002817cf511..63bd829c3b8 100644 --- a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py +++ b/moveit_ros/moveit_servo/launch/demo_twist.launch.py @@ -1,39 +1,42 @@ import os import launch 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 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() + ) -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 + # Get parameters for the Servo node 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 + # This filter parameter should be >1. Increase it for greater smoothing but slower motion. + low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} - moveit_config = ( - MoveItConfigsBuilder("moveit_resources_panda") - .robot_description( - file_path="config/panda.urdf.xacro", mappings=robot_description_mappings - ) - .to_moveit_configs() + # 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 @@ -67,9 +70,9 @@ def generate_servo_test_description( arguments=["panda_arm_controller", "-c", "/controller_manager"], ) - # Component nodes for tf and Servo - test_container = launch_ros.actions.ComposableNodeContainer( - name="test_servo_integration_container", + # 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", @@ -84,75 +87,33 @@ def generate_servo_test_description( package="tf2_ros", plugin="tf2_ros::StaticTransformBroadcasterNode", name="static_tf2_broadcaster", - parameters=[{"/child_frame_id": "panda_link0", "/frame_id": "world"}], + 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_main", - output="screen", + executable="demo_twist", 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", ) return launch.LaunchDescription( [ - launch.actions.DeclareLaunchArgument( - name="test_binary_dir", - description="Binary directory of package " - "containing test executables", - ), + rviz_node, 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(), + container, ] - ), { - "servo_node": servo_node, - "test_container": test_container, - "servo_gtest": servo_gtest, - "ros2_control_node": ros2_control_node, - } + ) 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 aac0882babc..00000000000 --- 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/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_monitor.cpp similarity index 92% rename from moveit_ros/moveit_servo/src/collision_check.cpp rename to moveit_ros/moveit_servo/src/collision_monitor.cpp index 3a3c8ede941..de8a409507f 100644 --- a/moveit_ros/moveit_servo/src/collision_check.cpp +++ b/moveit_ros/moveit_servo/src/collision_monitor.cpp @@ -39,12 +39,14 @@ #include -#include -// #include +#include -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.collision_check"); -static const double MIN_RECOMMENDED_COLLISION_RATE = 10; +namespace +{ +const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.collision_check"); +const double MIN_RECOMMENDED_COLLISION_RATE = 10; constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30 * 1000; // Milliseconds to throttle logs inside loops +} // namespace namespace moveit_servo { @@ -79,7 +81,7 @@ CollisionCheck::CollisionCheck(const rclcpp::Node::SharedPtr& node, collision_velocity_scale_pub_ = node_->create_publisher("~/collision_velocity_scale", rclcpp::SystemDefaultsQoS()); - current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + robot_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); } planning_scene_monitor::LockedPlanningSceneRO CollisionCheck::getLockedPlanningSceneRO() const @@ -113,14 +115,14 @@ void CollisionCheck::run() if (servo_params_.check_collisions) { // Update to the latest current state - current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); - current_state_->updateCollisionBodyTransforms(); + robot_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + robot_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_); + *robot_state_); scene_collision_distance_ = collision_result_.distance; collision_detected_ |= collision_result_.collision; collision_result_.print(); @@ -128,7 +130,7 @@ void CollisionCheck::run() 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()); + collision_request_, collision_result_, *robot_state_, getLockedPlanningSceneRO()->getAllowedCollisionMatrix()); self_collision_distance_ = collision_result_.distance; collision_detected_ |= collision_result_.collision; collision_result_.print(); diff --git a/moveit_ros/moveit_servo/src/command_processor.cpp b/moveit_ros/moveit_servo/src/command_processor.cpp new file mode 100644 index 00000000000..aca6fcd9ed5 --- /dev/null +++ b/moveit_ros/moveit_servo/src/command_processor.cpp @@ -0,0 +1,275 @@ +/******************************************************************************* + * 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_processor.hpp + * Project : moveit_servo + * Created : 04/06/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 +{ + +CommandProcessor::CommandProcessor(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const moveit::core::JointModelGroup* joint_model_group, + moveit::core::RobotStatePtr& current_state, servo::Params& servo_params, + StatusCode& servo_status) + : planning_scene_monitor_{ planning_scene_monitor } + , joint_model_group_{ joint_model_group } + , robot_state_{ current_state } + , servo_params_{ servo_params } + , servo_status_{ servo_status } +{ + num_joints_ = joint_model_group_->getActiveJointModelNames().size(); + + // Create PID controllers for pose tracking + rclcpp::WallRate controller_rate(1.0 / servo_params_.publish_period); + controller_period_ = controller_rate.period().count(); + controllers_ = createControllers(servo_params_); + RCLCPP_INFO_STREAM(LOGGER, "PID controllers created."); + + setIKSolver(); +} + +Eigen::VectorXd CommandProcessor::jointDeltaFromCommand(const JointJog& command) +{ + // Find the target joint position based on the commanded joint velocity + Eigen::VectorXd joint_poition_delta(num_joints_); + joint_poition_delta.setZero(); + + if (isValidCommand(command)) + { + // The incoming command should be in rad/s + joint_poition_delta = command * servo_params_.publish_period; + } + else + { + servo_status_ = StatusCode::INVALID; + RCLCPP_WARN_STREAM(LOGGER, "Invalid joint velocity command"); + } + return joint_poition_delta; +} + +Eigen::VectorXd CommandProcessor::jointDeltaFromCommand(const Twist& command) +{ + Eigen::VectorXd joint_position_delta(num_joints_); + joint_position_delta.setZero(); + Eigen::VectorXd cartesian_position_delta; + + std::string command_frame = command.frame_id; + const std::string planning_frame = servo_params_.planning_frame; + + const bool has_transform = transformExists(robot_state_, command_frame); + const bool valid_command = isValidCommand(command.velocities); + + if (has_transform && 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; + + // Compute the required change in joint angles. + if (ik_solver_) + { + // Use robot's IK solver to get joint position delta. + joint_position_delta = deltaFromIkSolver(cartesian_position_delta); + } + else + { + // Robot does not have an IK solver, use inverse Jacobian to compute IK. + 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(); + + joint_position_delta = pseudo_inverse * cartesian_position_delta; + } + + // Get velocity scaling information for singularity. + std::pair singularity_scaling_info = + velocityScalingFactorForSingularity(joint_model_group_, 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) + { + servo_status_ = singularity_scaling_info.second; + RCLCPP_WARN_STREAM(LOGGER, getStatusMessage()); + joint_position_delta *= singularity_scaling_info.first; + } + } + else + { + servo_status_ = StatusCode::INVALID; + if (!valid_command) + RCLCPP_WARN_STREAM(LOGGER, "Invalid twist command values."); + if (!has_transform) + RCLCPP_WARN_STREAM(LOGGER, "No transform available for command frame " << command_frame); + } + return joint_position_delta; +} + +Eigen::VectorXd CommandProcessor::jointDeltaFromCommand(const Pose& command) +{ + Eigen::VectorXd joint_position_delta(num_joints_); + joint_position_delta.setZero(); + + const std::string& command_frame = command.frame_id; + const std::string& planning_frame = servo_params_.planning_frame; + const bool has_transform = transformExists(robot_state_, command_frame); + const bool is_valid = isValidCommand(command.pose); + if (has_transform && is_valid) + { + Twist twist; + twist.frame_id = planning_frame; + twist.velocities.setZero(); + + // Compute linear and angular error. + const Eigen::Isometry3d ee_pose{ getEndEffectorPose() }; + const Eigen::Vector3d linear_delta = command.pose.translation() - ee_pose.translation(); + Eigen::Quaterniond q_current(ee_pose.rotation()), q_target(command.pose.rotation()); + Eigen::Quaterniond q_error = q_target * q_current.inverse(); + Eigen::AngleAxisd angle_axis_error(q_error); + Eigen::Vector3d angular_delta = angle_axis_error.axis() * angle_axis_error.angle(); + + // Compute the required twists. + twist.velocities[0] = controllers_["x"].computeCommand(linear_delta.x(), controller_period_); + twist.velocities[1] = controllers_["y"].computeCommand(linear_delta.y(), controller_period_); + twist.velocities[2] = controllers_["z"].computeCommand(linear_delta.z(), controller_period_); + twist.velocities[3] = controllers_["qx"].computeCommand(angular_delta.x(), controller_period_); + twist.velocities[4] = controllers_["qy"].computeCommand(angular_delta.y(), controller_period_); + twist.velocities[5] = controllers_["qz"].computeCommand(angular_delta.z(), controller_period_); + + joint_position_delta = jointDeltaFromCommand(twist); + } + else + { + servo_status_ = StatusCode::INVALID; + if (!has_transform) + RCLCPP_WARN_STREAM(LOGGER, "No transform available for command frame: " << command_frame); + } + return joint_position_delta; +} + +void CommandProcessor::setIKSolver() +{ + // 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()); + } + else + { + RCLCPP_INFO(LOGGER, "IK solver available for robot, will use it."); + } +} + +Eigen::VectorXd CommandProcessor::deltaFromIkSolver(const Eigen::VectorXd& cartesian_position_delta) +{ + Eigen::VectorXd delta_theta(num_joints_); + std::vector current_joint_positions(num_joints_); + + robot_state_->copyJointGroupPositions(joint_model_group_, current_joint_positions); + + const Eigen::Isometry3d base_to_tip_frame_transform = + robot_state_->getGlobalLinkTransform(ik_solver_->getBaseFrame()).inverse() * + robot_state_->getGlobalLinkTransform(ik_solver_->getTipFrame()); + + geometry_msgs::msg::Pose next_pose = poseFromCartesianDelta(cartesian_position_delta, 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, 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 < num_joints_; ++i) + { + delta_theta.coeffRef(i) = solution.at(i) - current_joint_positions[i]; + } + } + else + { + RCLCPP_WARN_STREAM(LOGGER, "Could not find IK solution for requested motion, got error code " << err.val); + } + + return delta_theta; +} + +StatusCode CommandProcessor::getStatus() +{ + return servo_status_; +} + +const std::string CommandProcessor::getStatusMessage() +{ + return SERVO_STATUS_CODE_MAP.at(servo_status_); +} + +const Eigen::Isometry3d CommandProcessor::getEndEffectorPose() +{ + // Robot base (panda_link0) to end-effector frame (panda_link8) + robot_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + return robot_state_->getGlobalLinkTransform(servo_params_.ee_frame_name); +} + +void CommandProcessor::resetControllers() +{ + for (auto& controller : controllers_) + { + controller.second.reset(); + } + RCLCPP_INFO_STREAM(LOGGER, "PID controllers have been reset"); +} + +} // 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 2f52cf16577..00000000000 --- 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 359f202523d..00000000000 --- 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 7bbe6c55c3e..816db0c2ebb 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -33,74 +33,424 @@ /* Title : servo.cpp * Project : moveit_servo - * Created : 3/9/2017 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + * Created : 17/05/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 -} // 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 { + +Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr& servo_param_listener) + : node_(node), servo_param_listener_{ servo_param_listener } +{ + servo_params_ = servo_param_listener_->get_params(); + + validateParams(servo_params_); + + createPlanningSceneMonitor(); + + // Create the collision checker + collision_checker_ = std::make_unique(node_, planning_scene_monitor_, servo_param_listener_); + if (servo_params_.check_collisions) + collision_checker_->start(); + + // Create collision velocity subscriber + collision_velocity_scale_sub_ = node_->create_subscription( + "~/collision_velocity_scale", rclcpp::SystemDefaultsQoS(), + [this](const std_msgs::msg::Float64::ConstSharedPtr& msg) { return collisionVelocityScaleCB(msg); }); + + // Get the robot state and joint model group info. + robot_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + 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 << '`'); + throw std::runtime_error("Invalid move group name"); + } + else + { + // Create the command processor to handle the different types of servo inputs. + command_processor_ = std::make_unique(planning_scene_monitor_, joint_model_group_, robot_state_, + servo_params_, servo_status_); + + // Get necessary information about joints + joint_names_ = joint_model_group_->getActiveJointModelNames(); + joint_bounds_ = joint_model_group_->getActiveJointModelsBounds(); + num_joints_ = joint_names_.size(); + } + + // Load the smoothing plugin + setSmoothingPlugin(); + + // Check if the tansforms to planning frame and end-effector frame exists. + if (!transformExists(robot_state_, servo_params_.planning_frame)) + { + servo_status_ = StatusCode::INVALID; + RCLCPP_ERROR_STREAM(LOGGER, "No transform available for planning frame " << servo_params_.planning_frame); + } + else if (!transformExists(robot_state_, servo_params_.ee_frame_name)) + { + servo_status_ = StatusCode::INVALID; + RCLCPP_ERROR_STREAM(LOGGER, "No transform available for end-effector frame " << servo_params_.ee_frame_name); + } + else + { + servo_status_ = StatusCode::NO_WARNING; + RCLCPP_INFO_STREAM(LOGGER, "Servo initialized successfully"); + } } -void Servo::start() +KinematicState Servo::getNextJointState(const ServoInput& command) { - if (!planning_scene_monitor_->getStateMonitor()->waitForCompleteState(servo_params_.move_group_name, - ROBOT_STATE_WAIT_TIME)) + // Update the parameters + if (servo_params_.enable_parameter_update) { - RCLCPP_ERROR(LOGGER, "Timeout waiting for current state"); - return; + updateParams(); } - servo_params_ = servo_param_listener_->get_params(); + // Set status to clear + servo_status_ = StatusCode::NO_WARNING; - // Crunch the numbers in this timer - servo_calcs_.start(); + // 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_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + 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_); - // Check collisions in this timer - if (servo_params_.check_collisions) - collision_checker_.start(); + // Update filter state + smoother_->reset(current_state.positions); + + // Compute the change in joint position due to the incoming command + Eigen::VectorXd joint_position_delta = jointDeltaFromCommand(command); + + // Continue rest of the computations only if the command was valid + if (servo_status_ != StatusCode::INVALID) + { + // Apply collision scaling to the joint position delta + 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; + } + 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 + 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. + target_joint_velocities *= + velocityScalingFactor(target_joint_velocities, joint_bounds_, servo_params_.override_velocity_scaling_factor); + + // 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 + 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; +} + +Eigen::VectorXd Servo::jointDeltaFromCommand(const ServoInput& command) +{ + Eigen::VectorXd target_joint_positions(num_joints_); + target_joint_positions.setZero(); // This should be set to current position. + + CommandType incoming_type = incomingCommandType(); + + if (incoming_type == CommandType::JOINT_JOG && command.index() == static_cast(incoming_type)) + { + target_joint_positions = command_processor_->jointDeltaFromCommand(std::get(command)); + } + else if (incoming_type == CommandType::TWIST && command.index() == static_cast(incoming_type)) + { + target_joint_positions = command_processor_->jointDeltaFromCommand(std::get(command)); + } + else if (incoming_type == CommandType::POSE && command.index() == static_cast(incoming_type)) + { + target_joint_positions = command_processor_->jointDeltaFromCommand(std::get(command)); + } + else + { + servo_status_ = StatusCode::INVALID; + RCLCPP_WARN_STREAM(LOGGER, "SERVO : Invalid command type, check if proper command type has been set."); + } + return target_joint_positions; +} + +KinematicState Servo::haltJoints(const std::vector& joints_to_halt, const KinematicState& current_state, + const KinematicState& target_state) +{ + KinematicState bounded_state(num_joints_); + bounded_state.joint_names = target_state.joint_names; + + std::stringstream halting_joint_names; + for (const int idx : joints_to_halt) + { + halting_joint_names << joint_names_[idx] + " "; + } + RCLCPP_WARN_STREAM(LOGGER, "Joint position limit reached on joints: " << halting_joint_names.str()); + + const bool all_joint_halt = + (incomingCommandType() == CommandType::JOINT_JOG && servo_params_.halt_all_joints_in_joint_mode) || + (incomingCommandType() == 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; } -void Servo::stop() +void Servo::collisionVelocityScaleCB(const std_msgs::msg::Float64::ConstSharedPtr& msg) { - servo_calcs_.stop(); - collision_checker_.stop(); + collision_velocity_scale_ = msg->data; } -bool Servo::getCommandFrameTransform(Eigen::Isometry3d& transform) +void Servo::createPlanningSceneMonitor() { - return servo_calcs_.getCommandFrameTransform(transform); + // 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"); + if (servo_params_.is_primary_planning_scene_monitor) + { + planning_scene_monitor_->providePlanningSceneService(); + } + else + { + planning_scene_monitor_->requestPlanningSceneState(); + } } -bool Servo::getCommandFrameTransform(geometry_msgs::msg::TransformStamped& transform) +void Servo::setSmoothingPlugin() { - return servo_calcs_.getCommandFrameTransform(transform); + // 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 + if (!smoother_->initialize(node_, planning_scene_monitor_->getRobotModel(), num_joints_)) + { + RCLCPP_ERROR(LOGGER, "Smoothing plugin could not be initialized"); + std::exit(EXIT_FAILURE); + } } -bool Servo::getEEFrameTransform(Eigen::Isometry3d& transform) +void Servo::updateParams() { - return servo_calcs_.getEEFrameTransform(transform); + 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 (robot_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 + params.robot_link_command_frame = servo_params_.robot_link_command_frame; + } + } + servo_params_ = params; + } } -bool Servo::getEEFrameTransform(geometry_msgs::msg::TransformStamped& transform) +StatusCode Servo::getStatus() { - return servo_calcs_.getEEFrameTransform(transform); + return servo_status_; } + +const std::string Servo::getStatusMessage() +{ + return SERVO_STATUS_CODE_MAP.at(servo_status_); +} + +CommandType Servo::incomingCommandType() +{ + return incoming_command_type_; +} + +void Servo::incomingCommandType(const CommandType& command_type) +{ + incoming_command_type_ = command_type; +} + +void Servo::validateParams(const servo::Params& servo_params) +{ + bool has_error = 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."); + has_error = true; + } + + 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."); + has_error = true; + } + + 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."); + has_error = true; + } + + 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."); + has_error = true; + } + + if (has_error) + { + throw std::runtime_error("Servo failed to initialize : Invalid parameter values"); + } +} + +const Eigen::Isometry3d Servo::getEndEffectorPose() +{ + return command_processor_->getEndEffectorPose(); +} + +void Servo::resetPoseControllers() +{ + command_processor_->resetControllers(); +} + +Pose Servo::toPlanningFrame(const Pose& command) +{ + if (command.frame_id != servo_params_.planning_frame) + { + Eigen::Isometry3d new_pose = command.pose; + auto tf_planning = robot_state_->getGlobalLinkTransform(command.frame_id); + // Modify only the position, rotation should be same. + new_pose.translation() = tf_planning.translation() + tf_planning.rotation() * command.pose.translation(); + return Pose{ servo_params_.planning_frame, new_pose }; + } + else + { + return command; + } +} + +Twist Servo::toPlanningFrame(const Twist& command) +{ + if (command.frame_id == servo_params_.planning_frame) + { + return command; + } + else + { + Twist transformed_twist = command; + // Transform the command to the MoveGroup planning frame + if (command.frame_id != servo_params_.planning_frame) + { + // We solve (planning_frame -> base -> command_frame) + // by computing (base->planning_frame)^-1 * (base->command_frame) + const Eigen::Isometry3d planning_frame_transfrom = + robot_state_->getGlobalLinkTransform(command.frame_id).inverse() * + robot_state_->getGlobalLinkTransform(servo_params_.planning_frame); + + // Apply the transformation to the command vector + transformed_twist.frame_id = servo_params_.planning_frame; + transformed_twist.velocities.head<3>() = planning_frame_transfrom.linear() * command.velocities.head<3>(); + transformed_twist.velocities.tail<3>() = planning_frame_transfrom.linear() * command.velocities.tail<3>(); + } + return transformed_twist; + } +} + } // 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 ef185c25d19..00000000000 --- 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 deleted file mode 100644 index 17d7c11a7eb..00000000000 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ /dev/null @@ -1,163 +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_node.cpp - * Project : moveit_servo - * Created : 12/31/2018 - * Author : Andy Zelenak - */ - -#include - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_node"); - -namespace moveit_servo -{ -ServoNode::ServoNode(const rclcpp::NodeOptions& options) - : node_{ std::make_shared("servo_node", options) } -{ - if (!options.use_intra_process_comms()) - { - RCLCPP_WARN_STREAM(LOGGER, "Intra-process communication is disabled, consider enabling it by adding: " - "\nextra_arguments=[{'use_intra_process_comms' : True}]\nto the Servo composable node " - "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(); - } - else - { - planning_scene_monitor_->requestPlanningSceneState(); - } - - // Create Servo - servo_ = std::make_unique(node_, planning_scene_monitor_, std::move(param_listener)); -} - -void ServoNode::validateParams(const servo::Params& servo_params) -{ - bool has_error = 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."); - has_error = true; - } - - 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."); - has_error = true; - } - - 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."); - has_error = true; - } - - 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."); - has_error = true; - } - - if (has_error) - { - throw std::runtime_error("Servo failed to initialize : Invalid parameter values"); - } -} - -void ServoNode::startCB(const std::shared_ptr& /* unused */, - const std::shared_ptr& response) -{ - servo_->start(); - response->success = true; -} - -void ServoNode::stopCB(const std::shared_ptr& /* unused */, - const std::shared_ptr& response) -{ - servo_->stop(); - response->success = true; -} -} // namespace moveit_servo - -// Register the component with class_loader -#include -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 81fd43227e3..00000000000 --- 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 b28fabb94a3..00000000000 --- 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 9c95a9801e6..00000000000 --- 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.cpp b/moveit_ros/moveit_servo/src/utils.cpp new file mode 100644 index 00000000000..40f4cb40ce4 --- /dev/null +++ b/moveit_ros/moveit_servo/src/utils.cpp @@ -0,0 +1,308 @@ +/******************************************************************************* + * 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 : 17/05/2023 + * Author : Andy Zelenak, V Mohammed Ibrahim + */ + +#include + +namespace moveit_servo +{ + +bool transformExists(const moveit::core::RobotStatePtr& current_state, const std::string& frame_name) +{ + bool has_transform = false; + if (current_state->knowsFrameTransform(frame_name)) + { + has_transform = true; + } + return has_transform; +} + +bool isValidCommand(const Eigen::VectorXd& command) +{ + bool is_valid = true; + for (const double& val : command) + { + if (std::isnan(val)) + { + is_valid = false; + break; + } + } + return is_valid; +} + +bool isValidCommand(const Eigen::Isometry3d& command) +{ + bool is_valid = true; + Eigen::Matrix3d I, R; + I.setIdentity(); + R = command.linear(); + + is_valid = I.isApprox(R.inverse() * R); + // Command is not vald if there is Nan + const Eigen::Vector3d translation = command.translation(); + const bool not_nan = (!std::isnan(translation.x()) && !std::isnan(translation.y()) && !std::isnan(translation.z())); + return is_valid && not_nan; +} + +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 + 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::pair +velocityScalingFactorForSingularity(const moveit::core::JointModelGroup* joint_model_group, + const moveit::core::RobotStatePtr& current_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; + + // 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. + size_t dims = target_delta_x.size(); + + // Get the current jacobian and compute SVD + Eigen::JacobiSVD current_svd = Eigen::JacobiSVD( + current_state->getJacobian(joint_model_group), Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::MatrixXd matrix_s = current_svd.singularValues().asDiagonal(); + + // Compute pseudo inverse + 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 + double scale = 100; + Eigen::VectorXd delta_x = vector_towards_singularity / scale; + + // Compute the new joint angles if we take the small step delta_x + Eigen::VectorXd next_joint_angles; + current_state->copyJointGroupPositions(joint_model_group, next_joint_angles); + next_joint_angles += pseudo_inverse * delta_x; + + // Compute the Jacobian SVD for the new robot state. + current_state->setJointGroupPositions(joint_model_group, next_joint_angles); + Eigen::JacobiSVD next_svd = Eigen::JacobiSVD( + current_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 velocityScalingFactor(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 < 0.01) + { + 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])[0]; + 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])[0]; + 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; +} + +std::map createControllers(const servo::Params& servo_params) +{ + std::map controllers; + + // Get the parameters + auto x_pid = servo_params.pose_tracking.x_pid; + auto y_pid = servo_params.pose_tracking.y_pid; + auto z_pid = servo_params.pose_tracking.z_pid; + auto q_pid = servo_params.pose_tracking.q_pid; + double windup_limit = servo_params.pose_tracking.windup_limit; + bool use_anti_windup = servo_params.pose_tracking.use_anti_windup; + + // Create the controllers + controllers["x"] = control_toolbox::Pid(x_pid.kp, x_pid.ki, x_pid.kd, windup_limit, -windup_limit, use_anti_windup); + controllers["y"] = control_toolbox::Pid(y_pid.kp, y_pid.ki, y_pid.kd, windup_limit, -windup_limit, use_anti_windup); + controllers["z"] = control_toolbox::Pid(z_pid.kp, z_pid.ki, z_pid.kd, windup_limit, -windup_limit, use_anti_windup); + controllers["qx"] = control_toolbox::Pid(q_pid.kp, q_pid.ki, q_pid.kd, windup_limit, -windup_limit, use_anti_windup); + controllers["qy"] = control_toolbox::Pid(q_pid.kp, q_pid.ki, q_pid.kd, windup_limit, -windup_limit, use_anti_windup); + controllers["qz"] = control_toolbox::Pid(q_pid.kp, q_pid.ki, q_pid.kd, windup_limit, -windup_limit, use_anti_windup); + + return controllers; +} + +} // 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 a0c8afdfe40..00000000000 --- 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 5c2a4889a26..00000000000 --- 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 007aa8002f6..00000000000 --- 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 2a6d28a78eb..00000000000 --- 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 769c256f5b3..00000000000 --- 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 6651b5c93dc..00000000000 --- 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", 1, 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 b9a0c779c56..00000000000 --- 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 5a8262ca351..00000000000 --- 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 86e260def96..00000000000 --- 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 1bb13065c02..00000000000 --- 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 f64451e6cdc..00000000000 --- 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; -}