diff --git a/urc_arm_moveit_config/config/WalliiArmV3.ros2_control.xacro b/urc_arm_moveit_config/config/WalliiArmV3.ros2_control.xacro index bc8171e7..cb46e04c 100644 --- a/urc_arm_moveit_config/config/WalliiArmV3.ros2_control.xacro +++ b/urc_arm_moveit_config/config/WalliiArmV3.ros2_control.xacro @@ -5,10 +5,11 @@ - - - - gazebo_ros2_control/GazeboSystem + + + gazebo_ros2_control/GazeboSystem + + diff --git a/urc_arm_moveit_config/config/WalliiArmV3.urdf.xacro b/urc_arm_moveit_config/config/WalliiArmV3.urdf.xacro index 74049dcb..d7b922b1 100644 --- a/urc_arm_moveit_config/config/WalliiArmV3.urdf.xacro +++ b/urc_arm_moveit_config/config/WalliiArmV3.urdf.xacro @@ -1,9 +1,13 @@ + + - + + + diff --git a/urc_arm_moveit_config/config/WalliiArmV3_simulated_config.yaml b/urc_arm_moveit_config/config/WalliiArmV3_simulated_config.yaml index 873bc12d..89daebbb 100644 --- a/urc_arm_moveit_config/config/WalliiArmV3_simulated_config.yaml +++ b/urc_arm_moveit_config/config/WalliiArmV3_simulated_config.yaml @@ -39,7 +39,7 @@ is_primary_planning_scene_monitor: false ## MoveIt properties move_group_name: arm # 'arm' for arm, 'gripper' for gripper -planning_frame: world # The MoveIt planning frame. Often 'base_link' or 'world' +planning_frame: world # world # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames ee_frame_name: leftgripper # The name of the end effector link, used to return the EE pose diff --git a/urc_arm_moveit_config/config/chomp_planning.yaml b/urc_arm_moveit_config/config/chomp_planning.yaml new file mode 100644 index 00000000..fa0d77c5 --- /dev/null +++ b/urc_arm_moveit_config/config/chomp_planning.yaml @@ -0,0 +1,12 @@ +planning_plugin: chomp_interface/CHOMPPlanner +enable_failure_recovery: true +jiggle_fraction: 0.05 +request_adapters: >- + default_planner_request_adapters/AddTimeOptimalParameterization + default_planner_request_adapters/ResolveConstraintFrames + default_planner_request_adapters/FixWorkspaceBounds + default_planner_request_adapters/FixStartStateBounds + default_planner_request_adapters/FixStartStateCollision + default_planner_request_adapters/FixStartStatePathConstraints +ridge_factor: 0.01 +start_state_max_bounds_error: 0.1 diff --git a/urc_arm_moveit_config/config/moveit.rviz b/urc_arm_moveit_config/config/moveit.rviz index f31651ed..1aaa81e1 100644 --- a/urc_arm_moveit_config/config/moveit.rviz +++ b/urc_arm_moveit_config/config/moveit.rviz @@ -27,7 +27,7 @@ Visualization Manager: Robot Alpha: 0.5 Value: true Global Options: - Fixed Frame: world + Fixed Frame: arm_base_link Tools: - Class: rviz_default_plugins/Interact - Class: rviz_default_plugins/MoveCamera @@ -43,7 +43,7 @@ Visualization Manager: Z: 0.30 Name: Current View Pitch: 0.5 - Target Frame: world + Target Frame: arm_base_link Yaw: -0.623 Window Geometry: Height: 975 diff --git a/urc_arm_moveit_config/launch/demo.launch.py b/urc_arm_moveit_config/launch/demo.launch.py index cc8e5bb9..431755e6 100644 --- a/urc_arm_moveit_config/launch/demo.launch.py +++ b/urc_arm_moveit_config/launch/demo.launch.py @@ -4,5 +4,5 @@ def generate_launch_description(): moveit_config = MoveItConfigsBuilder("WalliiArmV3", - package_name="walliiArm").to_moveit_configs() + package_name="urc_arm_moveit_config").to_moveit_configs() return generate_demo_launch(moveit_config) diff --git a/urc_arm_moveit_config/launch/move_group.launch.py b/urc_arm_moveit_config/launch/move_group.launch.py index 77e3cc3e..710d2808 100644 --- a/urc_arm_moveit_config/launch/move_group.launch.py +++ b/urc_arm_moveit_config/launch/move_group.launch.py @@ -4,5 +4,5 @@ def generate_launch_description(): moveit_config = MoveItConfigsBuilder("WalliiArmV3", - package_name="walliiArm").to_moveit_configs() + package_name="urc_arm_moveit_config").to_moveit_configs() return generate_move_group_launch(moveit_config) diff --git a/urc_arm_moveit_config/launch/moveit_rviz.launch.py b/urc_arm_moveit_config/launch/moveit_rviz.launch.py index b831c04d..4edf1470 100644 --- a/urc_arm_moveit_config/launch/moveit_rviz.launch.py +++ b/urc_arm_moveit_config/launch/moveit_rviz.launch.py @@ -4,5 +4,5 @@ def generate_launch_description(): moveit_config = MoveItConfigsBuilder("WalliiArmV3", - package_name="walliiArm").to_moveit_configs() + package_name="urc_arm_moveit_config").to_moveit_configs() return generate_moveit_rviz_launch(moveit_config) diff --git a/urc_arm_moveit_config/launch/rsp.launch.py b/urc_arm_moveit_config/launch/rsp.launch.py index e5f9f3e0..259ca141 100644 --- a/urc_arm_moveit_config/launch/rsp.launch.py +++ b/urc_arm_moveit_config/launch/rsp.launch.py @@ -4,5 +4,5 @@ def generate_launch_description(): moveit_config = MoveItConfigsBuilder("WalliiArmV3", - package_name="walliiArm").to_moveit_configs() + package_name="urc_arm_moveit_config").to_moveit_configs() return generate_rsp_launch(moveit_config) diff --git a/urc_arm_moveit_config/launch/setup_assistant.launch.py b/urc_arm_moveit_config/launch/setup_assistant.launch.py index 1e8f0819..1c020730 100644 --- a/urc_arm_moveit_config/launch/setup_assistant.launch.py +++ b/urc_arm_moveit_config/launch/setup_assistant.launch.py @@ -4,5 +4,5 @@ def generate_launch_description(): moveit_config = MoveItConfigsBuilder("WalliiArmV3", - package_name="walliiArm").to_moveit_configs() + package_name="urc_arm_moveit_config").to_moveit_configs() return generate_setup_assistant_launch(moveit_config) diff --git a/urc_arm_moveit_config/launch/spawn_controllers.launch.py b/urc_arm_moveit_config/launch/spawn_controllers.launch.py index e1b1f311..f942876a 100644 --- a/urc_arm_moveit_config/launch/spawn_controllers.launch.py +++ b/urc_arm_moveit_config/launch/spawn_controllers.launch.py @@ -4,5 +4,5 @@ def generate_launch_description(): moveit_config = MoveItConfigsBuilder("WalliiArmV3", - package_name="walliiArm").to_moveit_configs() + package_name="urc_arm_moveit_config").to_moveit_configs() return generate_spawn_controllers_launch(moveit_config) diff --git a/urc_arm_moveit_config/launch/static_virtual_joint_tfs.launch.py b/urc_arm_moveit_config/launch/static_virtual_joint_tfs.launch.py index aa5d1b81..da0654cb 100644 --- a/urc_arm_moveit_config/launch/static_virtual_joint_tfs.launch.py +++ b/urc_arm_moveit_config/launch/static_virtual_joint_tfs.launch.py @@ -4,5 +4,5 @@ def generate_launch_description(): moveit_config = MoveItConfigsBuilder("WalliiArmV3", - package_name="walliiArm").to_moveit_configs() + package_name="urc_arm_moveit_config").to_moveit_configs() return generate_static_virtual_joint_tfs_launch(moveit_config) diff --git a/urc_arm_moveit_config/launch/warehouse_db.launch.py b/urc_arm_moveit_config/launch/warehouse_db.launch.py index 6f07c77b..4c1a118b 100644 --- a/urc_arm_moveit_config/launch/warehouse_db.launch.py +++ b/urc_arm_moveit_config/launch/warehouse_db.launch.py @@ -4,5 +4,5 @@ def generate_launch_description(): moveit_config = MoveItConfigsBuilder("WalliiArmV3", - package_name="walliiArm").to_moveit_configs() + package_name="urc_arm_moveit_config").to_moveit_configs() return generate_warehouse_db_launch(moveit_config) diff --git a/urc_arm_moveit_config/package.xml b/urc_arm_moveit_config/package.xml index 97b64bca..51d483b1 100644 --- a/urc_arm_moveit_config/package.xml +++ b/urc_arm_moveit_config/package.xml @@ -18,14 +18,22 @@ ament_cmake + moveit_common controller_manager controller_manager_msgs moveit_core - moveit_ros_move_group + moveit_ros_occupancy_map_monitor + moveit_ros_perception moveit_ros_planning moveit_kinematics moveit_planners + moveit_planners_chomp moveit_simple_controller_manager + moveit_configs_utils + moveit_ros_move_group + moveit_ros_visualization + moveit_ros_warehouse + moveit_setup_assistant joint_state_publisher joint_state_publisher_gui tf2_ros @@ -34,11 +42,6 @@ We don't include them by default to prevent installing gazebo and all its dependencies. --> - moveit_configs_utils - moveit_ros_move_group - moveit_ros_visualization - moveit_ros_warehouse - moveit_setup_assistant robot_state_publisher rviz2 rviz_common diff --git a/urc_gazebo/config/joint_names_Arm URDF v1.yaml b/urc_gazebo/config/joint_names_Arm URDF v1.yaml deleted file mode 100644 index 993f6857..00000000 --- a/urc_gazebo/config/joint_names_Arm URDF v1.yaml +++ /dev/null @@ -1 +0,0 @@ -controller_joint_names: ['', 'shoulder_swivel', 'shoulder_hinge', 'elbow_hinge', 'elbow_swivel', 'wrist_hinge', 'wrist_swivel', ] diff --git a/urc_gazebo/urdf/WalliiArmV3.urdf b/urc_gazebo/urdf/WalliiArmV3.urdf index 5820d3f5..3cce7c49 100755 --- a/urc_gazebo/urdf/WalliiArmV3.urdf +++ b/urc_gazebo/urdf/WalliiArmV3.urdf @@ -555,24 +555,17 @@ true - - - - - + + + + gazebo_ros2_control/GazeboSystem + + ros2_control_demo_hardware/VelocityActuatorHardware - - - - 0.0 - - - - - + diff --git a/urc_gazebo/urdf/walli_prop.xacro b/urc_gazebo/urdf/walli_prop.xacro index 8b9dbf83..f9c88867 100644 --- a/urc_gazebo/urdf/walli_prop.xacro +++ b/urc_gazebo/urdf/walli_prop.xacro @@ -2,6 +2,8 @@ --> + + diff --git a/urc_manipulation/CMakeLists.txt b/urc_manipulation/CMakeLists.txt index 78270e51..384529d3 100644 --- a/urc_manipulation/CMakeLists.txt +++ b/urc_manipulation/CMakeLists.txt @@ -1,171 +1,50 @@ cmake_minimum_required(VERSION 3.5) project(urc_manipulation) -set(CMAKE_BUILD_TYPE "Release") - include(../cmake/default_settings.cmake) -# C++ Libraries ################################################# - -# Core C++ library for calculations and collision checking. -# Provides interface used by the component node. -set(SERVO_LIB_NAME moveit_servo_lib) -# C++ library containing the parameters initialization -# - This is intended to use with the component node when you -# - need to read the parameters loaded into servo in a separate -# - node. -set(SERVO_PARAM_LIB_NAME ${SERVO_LIB_NAME}_parameters) - -# Pose Tracking -set(POSE_TRACKING pose_tracking) - -# Component Nodes (Shared libraries) ############################ -set(SERVO_COMPONENT_NODE servo_node) -set(SERVO_CONTROLLER_INPUT servo_controller_input) - -# Executable Nodes ############################################## -set(SERVO_NODE_MAIN_NAME servo_node_main) -#set(POSE_TRACKING_DEMO_NAME servo_pose_tracking_demo) -#set(FAKE_SERVO_CMDS_NAME fake_command_publisher) - -################################################################# - -find_package(moveit_common REQUIRED) -moveit_package() - -set(THIS_PACKAGE_INCLUDE_DEPENDS - controller_manager - controller_manager_msgs - control_msgs - control_toolbox - geometry_msgs - moveit_core - moveit_msgs - moveit_ros_planning - pluginlib - rclcpp - rclcpp_components - sensor_msgs - std_msgs - std_srvs - tf2_eigen - trajectory_msgs -) - # find dependencies find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(control_msgs REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() include_directories( include ) -################### -## C++ Libraries ## -################### - -# This library provides a way of loading parameters for servo -add_library(${SERVO_PARAM_LIB_NAME} SHARED - src/servo_parameters.cpp - src/parameter_descriptor_builder.cpp +# Library creation +add_library(${PROJECT_NAME} SHARED + src/joy_to_servo_pub.cpp ) -set_target_properties(${SERVO_PARAM_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(${SERVO_PARAM_LIB_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) -# This library provides a C++ interface for sending realtime twist or joint commands to a robot -add_library(${SERVO_LIB_NAME} SHARED - # These files are used to produce differential motion - src/collision_check.cpp - src/enforce_limits.cpp - src/servo.cpp - src/servo_calcs.cpp - src/utilities.cpp +set(dependencies + rclcpp + rclcpp_components + sensor_msgs + geometry_msgs + std_srvs + moveit_msgs + control_msgs ) -set_target_properties(${SERVO_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(${SERVO_LIB_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(${SERVO_LIB_NAME} ${SERVO_PARAM_LIB_NAME}) - -add_library(${POSE_TRACKING} SHARED src/pose_tracking.cpp) -ament_target_dependencies(${POSE_TRACKING} ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(${POSE_TRACKING} ${SERVO_LIB_NAME}) - - -##################### -## Component Nodes ## -##################### -# Add and export library to run as a ROS node component, and receive commands via topics -add_library(${SERVO_COMPONENT_NODE} SHARED src/servo_node.cpp) -ament_target_dependencies(${SERVO_COMPONENT_NODE} ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(${SERVO_COMPONENT_NODE} ${SERVO_LIB_NAME}) -rclcpp_components_register_nodes(${SERVO_COMPONENT_NODE} "moveit_servo::ServoNode") - -# Add executable for using a controller -#add_library(${SERVO_CONTROLLER_INPUT} SHARED src/joy_to_servo_pub.cpp) -#ament_target_dependencies(${SERVO_CONTROLLER_INPUT} ${THIS_PACKAGE_INCLUDE_DEPENDS}) -#rclcpp_components_register_nodes(${SERVO_CONTROLLER_INPUT} "joy_to_servo_pub::JoyToServoPub") - -# Manual controls for the robotic arm -add_library(${SERVO_CONTROLLER_INPUT} SHARED - src/joy_to_servo_pub.cpp +ament_target_dependencies(${PROJECT_NAME} + ${dependencies} ) -ament_target_dependencies(${SERVO_CONTROLLER_INPUT} - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) # Node registration rclcpp_components_register_node( - ${SERVO_CONTROLLER_INPUT} + ${PROJECT_NAME} PLUGIN "joy_to_servo_pub::JoyToServoPub" - EXECUTABLE ${SERVO_CONTROLLER_INPUT}_JoyToServoPub -) - -###################### -## Executable Nodes ## -###################### - -# An executable node for the servo server -add_executable(${SERVO_NODE_MAIN_NAME} src/servo_node_main.cpp) -target_link_libraries(${SERVO_NODE_MAIN_NAME} ${SERVO_COMPONENT_NODE}) -ament_target_dependencies(${SERVO_NODE_MAIN_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -# An example of pose tracking -#add_executable(${POSE_TRACKING_DEMO_NAME} src/cpp_interface_demo/pose_tracking_demo.cpp) -#target_link_libraries(${POSE_TRACKING_DEMO_NAME} ${POSE_TRACKING}) -#ament_target_dependencies(${POSE_TRACKING_DEMO_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -# Add executable to publish fake servo commands for testing/demo purposes -#add_executable(${FAKE_SERVO_CMDS_NAME} test/publish_fake_jog_commands.cpp) -#ament_target_dependencies(${FAKE_SERVO_CMDS_NAME} -# rclcpp -# geometry_msgs -# std_srvs -#) - -############# -## Install ## -############# - -# Install Libraries -install( - TARGETS - ${SERVO_LIB_NAME} - ${SERVO_LIB_NAME}_parameters - ${POSE_TRACKING} - ${SERVO_COMPONENT_NODE} - ${SERVO_CONTROLLER_INPUT} - EXPORT export_${PROJECT_NAME} - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include + EXECUTABLE ${PROJECT_NAME}_JoyToServoPub ) # Install launch files. -install(DIRECTORY include/ DESTINATION include) install( DIRECTORY launch @@ -173,22 +52,14 @@ install( DESTINATION share/${PROJECT_NAME}/ ) -# Install Binaries +# Install library install(TARGETS - #${PROJECT_NAME} - ${SERVO_NODE_MAIN_NAME} - ${CPP_DEMO_NAME} - ${POSE_TRACKING_DEMO_NAME} - ${FAKE_SERVO_CMDS_NAME} + ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} ) -############# -## TESTING ## -############# - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the copyright linker @@ -202,8 +73,6 @@ endif() ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_export_dependencies(${dependencies}) ament_package() - - diff --git a/urc_manipulation/include/moveit_servo/collision_check.h b/urc_manipulation/include/moveit_servo/collision_check.h deleted file mode 100644 index bd081cc6..00000000 --- a/urc_manipulation/include/moveit_servo/collision_check.h +++ /dev/null @@ -1,122 +0,0 @@ -/******************************************************************************* - * Title : collision_check.h - * Project : moveit_servo - * Created : 1/11/2019 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -#pragma once - -#include - -#include - -#include -#include -#include -#include - -#include - -namespace moveit_servo -{ - class CollisionCheck - { -public: - /** \brief Constructor - * \param parameters: common settings of moveit_servo - * \param planning_scene_monitor: PSM should have scene monitor and state monitor - * already started when passed into this class - */ - CollisionCheck( - const rclcpp::Node::SharedPtr & node, const ServoParameters::SharedConstPtr & parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr & planning_scene_monitor); - - ~CollisionCheck() - { - if (timer_) { - timer_->cancel(); - } - } - - /** \brief start the Timer that regulates collision check rate */ - void start(); - - /** \brief Pause or unpause processing servo commands while keeping the timers alive */ - void setPaused(bool paused); - -private: - /** \brief Run one iteration of collision checking */ - void run(); - - /** \brief Get a read-only copy of the planning scene */ - planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const; - - // Pointer to the ROS node - const std::shared_ptr < rclcpp::Node > node_; - - // Parameters from yaml - const ServoParameters::SharedConstPtr parameters_; - - // Pointer to the collision environment - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - - // Robot state and collision matrix from planning scene - std::shared_ptr < moveit::core::RobotState > current_state_; - - // Scale robot velocity according to collision proximity and user-defined thresholds. - // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold. - // Proximity decreasing --> decelerate - double velocity_scale_ = 1; - double self_collision_distance_ = 0; - double scene_collision_distance_ = 0; - bool collision_detected_ = false; - bool paused_ = false; - - const double self_velocity_scale_coefficient_; - const double scene_velocity_scale_coefficient_; - - // collision request - collision_detection::CollisionRequest collision_request_; - collision_detection::CollisionResult collision_result_; - - // ROS - rclcpp::TimerBase::SharedPtr timer_; - double period_; // The loop period, in seconds - rclcpp::Publisher < std_msgs::msg::Float64 > ::SharedPtr collision_velocity_scale_pub_; - - mutable std::mutex joint_state_mutex_; - sensor_msgs::msg::JointState latest_joint_state_; - }; -} // namespace moveit_servo diff --git a/urc_manipulation/include/moveit_servo/enforce_limits.hpp b/urc_manipulation/include/moveit_servo/enforce_limits.hpp deleted file mode 100644 index d142be46..00000000 --- a/urc_manipulation/include/moveit_servo/enforce_limits.hpp +++ /dev/null @@ -1,60 +0,0 @@ -/******************************************************************************* - * Title : enforce_limits.h - * Project : moveit_servo - * Created : 7/5/2021 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, Tyler Weaver - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -#pragma once - -#include -#include - -#include - -namespace moveit_servo -{ -/** - * @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 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); - -} // namespace moveit_servo diff --git a/urc_manipulation/include/moveit_servo/make_shared_from_pool.h b/urc_manipulation/include/moveit_servo/make_shared_from_pool.h deleted file mode 100644 index c9e40c9a..00000000 --- a/urc_manipulation/include/moveit_servo/make_shared_from_pool.h +++ /dev/null @@ -1,57 +0,0 @@ -/******************************************************************************* - * Title : make_shared_from_pool.h - * Project : moveit_servo - * Created : 1/11/2019 - * Author : Tyler Weaver - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -#pragma once - -#include -#include - -namespace moveit -{ - namespace util - { -// Useful template for creating messages from a message pool - template < typename T > - std::shared_ptr < T > make_shared_from_pool() - { - using allocator_t = boost::fast_pool_allocator < std::shared_ptr < T >>; - return std::allocate_shared < T, allocator_t > (allocator_t()); - } - - } // namespace util -} // namespace moveit diff --git a/urc_manipulation/include/moveit_servo/parameter_descriptor_builder.hpp b/urc_manipulation/include/moveit_servo/parameter_descriptor_builder.hpp deleted file mode 100644 index b8b97c09..00000000 --- a/urc_manipulation/include/moveit_servo/parameter_descriptor_builder.hpp +++ /dev/null @@ -1,134 +0,0 @@ -// Copyright 2021 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 : Tyler Weaver - Desc : Creates a parameter descriptor message used to describe parameters - Title : parameter_descriptor_builder.hpp - Project : moveit_servo -*/ - -// TODO(823): Move this into a separate message_builder package - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace moveit_servo -{ -class ParameterDescriptorBuilder -{ - rcl_interfaces::msg::ParameterDescriptor msg_; - -public: - /** - * @brief Rcl_interfaces::msg::parameterdescriptor conversion operator. - */ - operator rcl_interfaces::msg::ParameterDescriptor() const - { - return msg_; - } - - /** - * @brief Set the type - * - * @param[in] type The type - * - * @return Reference to this object - */ - ParameterDescriptorBuilder & type(uint8_t type); - - /** - * @brief Set the description string - * - * @param[in] description The description - * - * @return Reference to this object - */ - ParameterDescriptorBuilder & description(const std::string & description); - - /** - * @brief Set the additional constraints string (a description of any additional constraints which cannot be - * expressed with the available parameter constraints) - * - * @param[in] additional_constraints The additional constraints - * - * @return Reference to this object - */ - ParameterDescriptorBuilder & additionalConstraints(const std::string & additional_constraints); - - /** - * @brief Sets the read only flag - * - * @param[in] read_only The read only flag - * - * @return Reference to this object - */ - ParameterDescriptorBuilder & readOnly(bool read_only); - - /** - * @brief Set the dynamic typing flag (rolling only) - * - * @param[in] dynamic_typing The dynamic typing flag - * - * @return Reference to this object - */ - ParameterDescriptorBuilder & dynamicTyping(bool dynamic_typing); - - /** - * @brief Set floating point range - * - * @param[in] from_value The from value - * @param[in] to_value To value - * @param[in] step The step - * - * @return Reference to this object - */ - ParameterDescriptorBuilder & floatingPointRange( - double from_value = std::numeric_limits::min(), - double to_value = std::numeric_limits::max(), double step = 0); - - /** - * @brief Set the integer range - * - * @param[in] from_value The from value - * @param[in] to_value To value - * @param[in] step The step - * - * @return Reference to this object - */ - ParameterDescriptorBuilder & integerRange( - int64_t from_value = std::numeric_limits::min(), - int64_t to_value = std::numeric_limits::max(), int64_t step = 0); -}; - -} // namespace moveit_servo diff --git a/urc_manipulation/include/moveit_servo/pose_tracking.h b/urc_manipulation/include/moveit_servo/pose_tracking.h deleted file mode 100644 index 8c070c13..00000000 --- a/urc_manipulation/include/moveit_servo/pose_tracking.h +++ /dev/null @@ -1,208 +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 < PoseTrackingStatusCode, std::string > 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, - const ServoParameters::SharedConstPtr & servo_parameters, - 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 < moveit_servo::Servo > 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 < control_toolbox::Pid > & 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_; - moveit_servo::ServoParameters::SharedConstPtr 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 < geometry_msgs::msg::TwistStamped > ::SharedPtr twist_stamped_pub_; - - std::vector < control_toolbox::Pid > cartesian_position_pids_; - std::vector < control_toolbox::Pid > 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 < geometry_msgs::msg::PoseStamped > ::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 < bool > stop_requested_; - - std::optional < double > angular_error_; - }; - -// using alias - using PoseTrackingPtr = std::shared_ptr < PoseTracking >; -} // namespace moveit_servo diff --git a/urc_manipulation/include/moveit_servo/servo.h b/urc_manipulation/include/moveit_servo/servo.h deleted file mode 100644 index 13c0a46a..00000000 --- a/urc_manipulation/include/moveit_servo/servo.h +++ /dev/null @@ -1,109 +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 ServoParameters::SharedConstPtr & parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr & planning_scene_monitor); - - ~Servo(); - - /** \brief start servo node */ - void start(); - - /** \brief Pause or unpause processing servo commands while keeping the timers alive */ - void setPaused(bool paused); - - /** - * 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); - - /** \brief Get the parameters used by servo node. */ - const ServoParameters::SharedConstPtr & getParameters() const; - - // Give test access to private/protected methods - friend class ServoFixture; - -private: - // Pointer to the collision environment - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - - // The stored servo parameters - ServoParameters::SharedConstPtr parameters_; - - ServoCalcs servo_calcs_; - CollisionCheck collision_checker_; - }; - -// ServoPtr using alias - using ServoPtr = std::shared_ptr < Servo >; - -} // namespace moveit_servo diff --git a/urc_manipulation/include/moveit_servo/servo_calcs.h b/urc_manipulation/include/moveit_servo/servo_calcs.h deleted file mode 100644 index 55fa13c9..00000000 --- a/urc_manipulation/include/moveit_servo/servo_calcs.h +++ /dev/null @@ -1,385 +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 -#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 std::shared_ptr < const moveit_servo::ServoParameters > ¶meters, - const planning_scene_monitor::PlanningSceneMonitorPtr & planning_scene_monitor); - - ~ServoCalcs(); - - /** \brief Start the timer where we do work and publish outputs */ - void start(); - - /** - * 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); - - /** \brief Pause or unpause processing servo commands while keeping the timers alive */ - void setPaused(bool paused); - -protected: - /** \brief Run the main calculation loop */ - void mainCalcLoop(); - - /** \brief Do calculations for a single iteration. Publish one outgoing command */ - void calculateSingleIteration(); - - /** \brief Stop the currently running thread */ - void stop(); - - /** \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); - - /** \brief Parse the incoming joint msg for the joints of our MoveGroup */ - void updateJoints(); - - /** - * 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 < const moveit::core::JointModel * > & joints_to_halt) const; - - /** \brief Avoid overshooting joint limits - \return Vector of the joints that would move farther past position margin limits - */ - std::vector < const moveit::core::JointModel * > enforcePositionLimits( - sensor_msgs::msg::JointState & joint_state) 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 - */ - bool internalServoUpdate( - Eigen::ArrayXd & delta_theta, trajectory_msgs::msg::JointTrajectory & joint_trajectory, - const ServoType servo_type); - - /** \brief Joint-wise update of a sensor_msgs::msg::JointState with given delta's - * Also filters and calculates the previous velocity - * @param delta_theta Eigen vector of joint delta's - * @param joint_state The joint state msg being updated - * @param previous_vel Eigen vector of previous velocities being updated - * @return Returns false if there is a problem, true otherwise - */ - bool applyJointUpdate( - const Eigen::ArrayXd & delta_theta, - sensor_msgs::msg::JointState & joint_state); - - /** \brief Gazebo simulations have very strict message timestamp requirements. - * Satisfy Gazebo by stuffing multiple messages into one. - */ - void insertRedundantPointsIntoTrajectory( - trajectory_msgs::msg::JointTrajectory & joint_trajectory, int count) const; - - /** - * Remove the Jacobian row and the delta-x element of one Cartesian dimension, to take advantage of task redundancy - * - * @param matrix The Jacobian matrix. - * @param delta_x Vector of Cartesian delta commands, should be the same size as matrix.rows() - * @param row_to_remove Dimension that will be allowed to drift, e.g. row_to_remove = 2 allows z-translation drift. - */ - void removeDimension( - Eigen::MatrixXd & matrix, Eigen::VectorXd & delta_x, - unsigned int row_to_remove) const; - - /** - * Removes all of the drift dimensions from the jacobian and delta-x element - * - * @param matrix The Jacobian matrix. - * @param delta_x Vector of Cartesian delta commands, should be the same size as matrix.rows() - */ - void removeDriftDimensions(Eigen::MatrixXd & matrix, Eigen::VectorXd & delta_x); - - /** - * Uses control_dimensions_ to set the incoming twist command values to 0 in uncontrolled directions - * - * @param command TwistStamped msg being used in the Cartesian calcs process - */ - void enforceControlDimensions(geometry_msgs::msg::TwistStamped & command); - - /* \brief Callback for joint subsription */ - void jointStateCB(const sensor_msgs::msg::JointState::SharedPtr msg); - - /* \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); - - /** - * Allow drift in certain dimensions. For example, may allow the wrist to rotate freely. - * This can help avoid singularities. - * - * @param request the service request - * @param response the service response - * @return true if the adjustment was made - */ - void changeDriftDimensions( - const std::shared_ptr < moveit_msgs::srv::ChangeDriftDimensions::Request > & req, - const std::shared_ptr < moveit_msgs::srv::ChangeDriftDimensions::Response > & res); - - /** \brief Start the main calculation timer */ - // Service callback for changing servoing dimensions - void changeControlDimensions( - const std::shared_ptr < moveit_msgs::srv::ChangeControlDimensions::Request > & req, - const std::shared_ptr < moveit_msgs::srv::ChangeControlDimensions::Response > & res); - - /** \brief Service callback to reset Servo status, e.g. so the arm can move again after a collision */ - bool resetServoStatus( - const std::shared_ptr < std_srvs::srv::Empty::Request > & req, - const std::shared_ptr < std_srvs::srv::Empty::Response > & res); - - // Pointer to the ROS node - std::shared_ptr < rclcpp::Node > node_; - - // Parameters from yaml - const std::shared_ptr < const moveit_servo::ServoParameters > parameters_; - - // Pointer to the collision environment - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - - // Track the number of cycles during which motion has not occurred. - // Will avoid re-publishing zero velocities endlessly. - int zero_velocity_count_ = 0; - - // 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; - - // Nonzero status flags - bool have_nonzero_twist_stamped_ = false; - bool have_nonzero_joint_command_ = false; - bool have_nonzero_command_ = 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_; - - // (mutex protected below) - // internal_joint_state_ is used in servo calculations. It shouldn't be relied on to be accurate. - // original_joint_state_ is the same as incoming_joint_state_ except it only contains the joints the servo node acts - // on. - sensor_msgs::msg::JointState internal_joint_state_, original_joint_state_; - std::map < std::string, std::size_t > joint_state_name_map_; - - // Smoothing algorithm (loads a plugin) - std::shared_ptr < online_signal_smoothing::SmoothingBaseClass > smoother_; - - trajectory_msgs::msg::JointTrajectory::SharedPtr last_sent_command_; - - // ROS - rclcpp::Subscription < sensor_msgs::msg::JointState > ::SharedPtr joint_state_sub_; - rclcpp::Subscription < geometry_msgs::msg::TwistStamped > ::SharedPtr twist_stamped_sub_; - rclcpp::Subscription < control_msgs::msg::JointJog > ::SharedPtr joint_cmd_sub_; - rclcpp::Subscription < std_msgs::msg::Float64 > ::SharedPtr collision_velocity_scale_sub_; - rclcpp::Publisher < std_msgs::msg::Int8 > ::SharedPtr status_pub_; - rclcpp::Publisher < trajectory_msgs::msg::JointTrajectory > - ::SharedPtr trajectory_outgoing_cmd_pub_; - rclcpp::Publisher < std_msgs::msg::Float64MultiArray > ::SharedPtr multiarray_outgoing_cmd_pub_; - rclcpp::Service < moveit_msgs::srv::ChangeControlDimensions > - ::SharedPtr control_dimensions_server_; - rclcpp::Service < moveit_msgs::srv::ChangeDriftDimensions > - ::SharedPtr drift_dimensions_server_; - rclcpp::Service < std_srvs::srv::Empty > ::SharedPtr reset_servo_status_; - - // Main tracking / result publisher loop - std::thread thread_; - bool stop_requested_; - std::atomic < bool > done_stopping_; - - // Status - StatusCode status_ = StatusCode::NO_WARNING; - std::atomic < bool > paused_; - bool twist_command_is_stale_ = false; - bool joint_command_is_stale_ = false; - bool ok_to_publish_ = false; - double collision_velocity_scale_ = 1.0; - - // Use ArrayXd type to enable more coefficient-wise operations - Eigen::ArrayXd delta_theta_; - - const int gazebo_redundant_message_count_ = 30; - - unsigned int num_joints_; - - // True -> allow drift in this dimension. In the command frame. [x, y, z, roll, pitch, yaw] - std::array < bool, 6 > drift_dimensions_ = {{false, false, false, false, false, false}}; - - // The dimensions to control. In the command frame. [x, y, z, roll, pitch, yaw] - std::array < bool, 6 > control_dimensions_ = {{true, true, true, true, true, true}}; - - // 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); - bool latest_twist_cmd_is_nonzero_ = false; - bool latest_joint_cmd_is_nonzero_ = false; - - // input condition variable used for low latency mode - std::condition_variable input_cv_; - bool new_input_cmd_ = false; - - // dynamic parameters - std::string robot_link_command_frame_; - rcl_interfaces::msg::SetParametersResult robotLinkCommandFrameCallback( - const rclcpp::Parameter & parameter); - - // Load a smoothing plugin - pluginlib::ClassLoader < online_signal_smoothing::SmoothingBaseClass > smoothing_loader_; - - kinematics::KinematicsBaseConstPtr ik_solver_; - Eigen::Isometry3d ik_base_to_tip_frame_; - bool use_inv_jacobian_ = false; - }; -} // namespace moveit_servo diff --git a/urc_manipulation/include/moveit_servo/servo_node.h b/urc_manipulation/include/moveit_servo/servo_node.h deleted file mode 100644 index 2b69d40a..00000000 --- a/urc_manipulation/include/moveit_servo/servo_node.h +++ /dev/null @@ -1,91 +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 - -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 < rclcpp::Node > node_; - std::unique_ptr < moveit_servo::Servo > servo_; - std::shared_ptr < tf2_ros::Buffer > tf_buffer_; - std::shared_ptr < planning_scene_monitor::PlanningSceneMonitor > planning_scene_monitor_; - - /** \brief Start the servo loop. Must be called once to begin Servoing. */ - void startCB( - const std::shared_ptr < std_srvs::srv::Trigger::Request > & request, - const std::shared_ptr < std_srvs::srv::Trigger::Response > & response); - rclcpp::Service < std_srvs::srv::Trigger > ::SharedPtr start_servo_service_; - - /** \brief Stop the servo loop. This involves more overhead than pauseCB, e.g. it clears the planning scene monitor. - * We recommend using pauseCB/unpauseCB if you will resume the Servo loop soon. - */ - void stopCB( - const std::shared_ptr < std_srvs::srv::Trigger::Request > & request, - const std::shared_ptr < std_srvs::srv::Trigger::Response > & response); - rclcpp::Service < std_srvs::srv::Trigger > ::SharedPtr stop_servo_service_; - - /** \brief Pause the servo loop but continue monitoring joint state so we can resume easily. */ - void pauseCB( - const std::shared_ptr < std_srvs::srv::Trigger::Request > & request, - const std::shared_ptr < std_srvs::srv::Trigger::Response > & response); - rclcpp::Service < std_srvs::srv::Trigger > ::SharedPtr pause_servo_service_; - - /** \brief Resume the servo loop after pauseCB has been called. */ - void unpauseCB( - const std::shared_ptr < std_srvs::srv::Trigger::Request > & request, - const std::shared_ptr < std_srvs::srv::Trigger::Response > & response); - rclcpp::Service < std_srvs::srv::Trigger > ::SharedPtr unpause_servo_service_; - }; -} // namespace moveit_servo diff --git a/urc_manipulation/include/moveit_servo/servo_parameters.h b/urc_manipulation/include/moveit_servo/servo_parameters.h deleted file mode 100644 index 0e855f3a..00000000 --- a/urc_manipulation/include/moveit_servo/servo_parameters.h +++ /dev/null @@ -1,178 +0,0 @@ -/******************************************************************************* - * Title : servo_parameters.h - * Project : moveit_servo - * Created : 1/11/2019 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -#pragma once - -#include -#include -#include -#include - -#include -namespace moveit_servo -{ - using SetParameterCallbackType = std::function < rcl_interfaces::msg::SetParametersResult( - const rclcpp::Parameter &) >; - -// ROS params to be read. See the yaml file in /config for a description of each. - struct ServoParameters - { - using SharedConstPtr = std::shared_ptr < const ServoParameters >; - - // Parameter namespace - const std::string ns; - - // ROS Parameters - // Note that all of these are effectively const because the only way to create one of these - // is as a shared_ptr to a constant struct. - bool use_gazebo {false}; - std::string status_topic {"~/status"}; - // Properties of incoming commands - std::string cartesian_command_in_topic {"~/delta_twist_cmds"}; - std::string joint_command_in_topic {"~/delta_joint_cmds"}; - std::string robot_link_command_frame {"arm_base_link"}; - std::string command_in_type {"unitless"}; - double linear_scale {0.4}; - double rotational_scale {0.8}; - double joint_scale {0.5}; - // Properties of Servo calculations - double override_velocity_scaling_factor {0.0}; - // Properties of outgoing commands - std::string command_out_topic {"/arm_controller/joint_trajectory"}; - double publish_period {0.034}; - std::string command_out_type {"trajectory_msgs/JointTrajectory"}; - bool publish_joint_positions {true}; - bool publish_joint_velocities {true}; - bool publish_joint_accelerations {false}; - // Plugins for smoothing outgoing commands - std::string joint_topic {"/joint_states"}; - std::string smoothing_filter_plugin_name {"online_signal_smoothing::ButterworthFilterPlugin"}; - // MoveIt properties - std::string move_group_name {"arm"}; - std::string planning_frame {"arm_base_link"}; - std::string ee_frame_name {"leftgripper"}; - bool is_primary_planning_scene_monitor = {true}; - std::string monitored_planning_scene_topic { - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC - }; - // Stopping behaviour - double incoming_command_timeout {0.1}; - int num_outgoing_halt_msgs_to_publish {4}; - bool halt_all_joints_in_joint_mode {true}; - bool halt_all_joints_in_cartesian_mode {true}; - // Configure handling of singularities and joint limits - double lower_singularity_threshold {17.0}; - double hard_stop_singularity_threshold {30.0}; - double leaving_singularity_threshold_multiplier {2.0}; - double joint_limit_margin {0.1}; - bool low_latency_mode {false}; - // Collision checking - bool check_collisions {true}; - double collision_check_rate {10.0}; - double self_collision_proximity_threshold {0.01}; - double scene_collision_proximity_threshold {0.02}; - - /** - * Declares, reads, and validates parameters used for moveit_servo - * @param node Shared ptr to node that will the parameters will be declared in. Params should be defined in - * launch/config files - * @param logger Logger for outputting warnings about the parameters - * @param parameters The set up parameters that will be updated. After this call, they can be used to start a Servo - * instance - * @param ns Parameter namespace (as loaded in launch files). Defaults to "moveit_servo" but can be changed to allow - * multiple arms/instances - * @param dynamic_parameters Enable dynamic parameter handling. (default: true) - * @return std::shared_ptr if all parameters were loaded and verified successfully, nullptr otherwise - */ - static SharedConstPtr makeServoParameters( - const rclcpp::Node::SharedPtr & node, const std::string & ns = "moveit_servo", - bool dynamic_parameters = true); - - /** - * Register a callback for a parameter set event. - * Note that these callbacks do not change any of the parameters struct. - * Use a local variable for tracking the state of the dynamic parameter after initial bringup. - * @param name Name of parameter (key used for callback in map) - * @param callback function to call when parameter is changed - */ - [[nodiscard]] bool registerSetParameterCallback( - const std::string & name, - const SetParameterCallbackType & callback) const - { - if (callback_handler_) { - const std::lock_guard < std::mutex > guard {callback_handler_->mutex_}; - callback_handler_->set_parameter_callbacks_[name].push_back(callback); - return true; - } - return false; - } - static ServoParameters get( - const std::string & ns, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters); - static std::optional < ServoParameters > validate(ServoParameters parameters); - -private: - // Private constructor because we only want this object to be created through the builder method makeServoParameters - ServoParameters() - { - } - - struct CallbackHandler - { - // callback handler for the on set parameters callback - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr - on_set_parameters_callback_handler_; - - // mutable so the callback can be registered objects that have const versions of this struct - mutable std::mutex mutex_; - mutable std::map < std::string, - std::vector < SetParameterCallbackType >> set_parameter_callbacks_; - - // For registering with add_on_set_parameters_callback after initializing data - rcl_interfaces::msg::SetParametersResult setParametersCallback( - const std::vector < rclcpp::Parameter > & parameters); - }; - - std::shared_ptr < CallbackHandler > callback_handler_; - - static void declare( - const std::string & ns, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters); - }; - -} // namespace moveit_servo diff --git a/urc_manipulation/include/moveit_servo/servo_server.h b/urc_manipulation/include/moveit_servo/servo_server.h deleted file mode 100644 index b3915865..00000000 --- a/urc_manipulation/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/urc_manipulation/include/moveit_servo/status_codes.h b/urc_manipulation/include/moveit_servo/status_codes.h deleted file mode 100644 index dd73b6f3..00000000 --- a/urc_manipulation/include/moveit_servo/status_codes.h +++ /dev/null @@ -1,69 +0,0 @@ -/******************************************************************************* - * 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 - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -#pragma once - -#include -#include - -namespace moveit_servo -{ - enum class StatusCode: int8_t - { - INVALID = -1, - NO_WARNING = 0, - DECELERATE_FOR_APPROACHING_SINGULARITY = 1, - HALT_FOR_SINGULARITY = 2, - DECELERATE_FOR_COLLISION = 3, - HALT_FOR_COLLISION = 4, - JOINT_BOUND = 5, - DECELERATE_FOR_LEAVING_SINGULARITY = 6 - }; - - const std::unordered_map < StatusCode, std::string > SERVO_STATUS_CODE_MAP( - {{StatusCode::INVALID, "Invalid"}, - {StatusCode::NO_WARNING, "No warnings"}, - {StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY, - "Moving closer to a singularity, decelerating"}, - {StatusCode::HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop"}, - {StatusCode::DECELERATE_FOR_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"}}); -} // namespace moveit_servo diff --git a/urc_manipulation/include/moveit_servo/utilities.h b/urc_manipulation/include/moveit_servo/utilities.h deleted file mode 100644 index a5bdbe69..00000000 --- a/urc_manipulation/include/moveit_servo/utilities.h +++ /dev/null @@ -1,83 +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 - -namespace moveit_servo -{ -// Helper function for detecting zeroed message - bool isNonZero(const geometry_msgs::msg::TwistStamped & msg); - -// Helper function for detecting zeroed message - bool isNonZero(const control_msgs::msg::JointJog & msg); - -// 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] clock A ROS clock, for logging - * @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 < Eigen::MatrixXd > & svd, - const Eigen::MatrixXd & pseudo_inverse, - const double hard_stop_singularity_threshold, - const double lower_singularity_threshold, - const double leaving_singularity_threshold_multiplier, rclcpp::Clock & clock, - moveit::core::RobotStatePtr & current_state, StatusCode & status); - -} // namespace moveit_servo diff --git a/urc_manipulation/package.xml b/urc_manipulation/package.xml index 1fecec58..8970efdd 100644 --- a/urc_manipulation/package.xml +++ b/urc_manipulation/package.xml @@ -27,6 +27,7 @@ tf2_eigen trajectory_msgs + moveit_ros gripper_controllers joint_state_broadcaster joint_trajectory_controller diff --git a/urc_manipulation/src/collision_check.cpp b/urc_manipulation/src/collision_check.cpp deleted file mode 100644 index 8629964c..00000000 --- a/urc_manipulation/src/collision_check.cpp +++ /dev/null @@ -1,165 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -/* Title : collision_check.cpp - * Project : moveit_servo - * Created : 1/11/2019 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - */ - -#include - -#include -// #include - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.collision_check"); -static const double MIN_RECOMMENDED_COLLISION_RATE = 10; -constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30 * 1000; // Milliseconds to throttle logs inside loops - -namespace moveit_servo -{ -// Constructor for the class that handles collision checking -CollisionCheck::CollisionCheck( - const rclcpp::Node::SharedPtr & node, const ServoParameters::SharedConstPtr & parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr & planning_scene_monitor) -: node_(node) - , parameters_(parameters) - , planning_scene_monitor_(planning_scene_monitor) - , self_velocity_scale_coefficient_(-log(0.001) / parameters->self_collision_proximity_threshold) - , scene_velocity_scale_coefficient_(-log(0.001) / parameters->scene_collision_proximity_threshold) - , period_(1. / parameters->collision_check_rate) -{ - // Init collision request - collision_request_.group_name = parameters_->move_group_name; - collision_request_.distance = true; // enable distance-based collision checking - collision_request_.contacts = true; // Record the names of collision pairs - - if (parameters_->collision_check_rate < MIN_RECOMMENDED_COLLISION_RATE) { - auto & clk = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE( - LOGGER, clk, ROS_LOG_THROTTLE_PERIOD, - "Collision check rate is low, increase it in yaml file if CPU allows"); - } - - // ROS pubs/subs - collision_velocity_scale_pub_ = - node_->create_publisher( - "~/collision_velocity_scale", - rclcpp::SystemDefaultsQoS()); - - current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); -} - -planning_scene_monitor::LockedPlanningSceneRO CollisionCheck::getLockedPlanningSceneRO() const -{ - return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); -} - -void CollisionCheck::start() -{ - timer_ = - node_->create_wall_timer(std::chrono::duration(period_), [this]() {return run();}); -} - -void CollisionCheck::run() -{ - if (paused_) { - return; - } - - // Update to the latest current state - current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); - current_state_->updateCollisionBodyTransforms(); - collision_detected_ = false; - - // Do a timer-safe distance-based collision detection - collision_result_.clear(); - getLockedPlanningSceneRO()->getCollisionEnv()->checkRobotCollision( - collision_request_, collision_result_, - *current_state_); - scene_collision_distance_ = collision_result_.distance; - collision_detected_ |= collision_result_.collision; - collision_result_.print(); - - collision_result_.clear(); - // Self-collisions and scene collisions are checked separately so different thresholds can be used - getLockedPlanningSceneRO()->getCollisionEnvUnpadded()->checkSelfCollision( - collision_request_, collision_result_, *current_state_, - getLockedPlanningSceneRO()->getAllowedCollisionMatrix()); - self_collision_distance_ = collision_result_.distance; - collision_detected_ |= collision_result_.collision; - collision_result_.print(); - - velocity_scale_ = 1; - // If we're definitely in collision, stop immediately - if (collision_detected_) { - velocity_scale_ = 0; - } else { - // If we are far from a collision, velocity_scale should be 1. - // If we are very close to a collision, velocity_scale should be ~zero. - // When scene_collision_proximity_threshold is breached, start decelerating exponentially. - if (scene_collision_distance_ < parameters_->scene_collision_proximity_threshold) { - // velocity_scale = e ^ k * (collision_distance - threshold) - // k = - ln(0.001) / collision_proximity_threshold - // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. - // velocity_scale should equal 0.001 when collision_distance is at zero. - velocity_scale_ = std::min( - velocity_scale_, - exp( - scene_velocity_scale_coefficient_ * - (scene_collision_distance_ - parameters_->scene_collision_proximity_threshold))); - } - - if (self_collision_distance_ < parameters_->self_collision_proximity_threshold) { - velocity_scale_ = - std::min( - velocity_scale_, exp( - self_velocity_scale_coefficient_ * - (self_collision_distance_ - parameters_->self_collision_proximity_threshold))); - } - } - - // publish message - { - auto msg = std::make_unique(); - msg->data = velocity_scale_; - collision_velocity_scale_pub_->publish(std::move(msg)); - } -} - -void CollisionCheck::setPaused(bool paused) -{ - paused_ = paused; -} - -} // namespace moveit_servo diff --git a/urc_manipulation/src/enforce_limits.cpp b/urc_manipulation/src/enforce_limits.cpp deleted file mode 100644 index 2ae2c9a7..00000000 --- a/urc_manipulation/src/enforce_limits.cpp +++ /dev/null @@ -1,107 +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 : enforce_limits.cpp - * Project : moveit_servo - * Created : 7/5/2021 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson, Tyler Weaver - */ - -#include -#include - -#include -#include - -namespace moveit_servo -{ -namespace -{ -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; -} - -} // namespace - -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()); - } -} - -} // namespace moveit_servo diff --git a/urc_manipulation/src/parameter_descriptor_builder.cpp b/urc_manipulation/src/parameter_descriptor_builder.cpp deleted file mode 100644 index a1c5070d..00000000 --- a/urc_manipulation/src/parameter_descriptor_builder.cpp +++ /dev/null @@ -1,98 +0,0 @@ -// Copyright 2021 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 : Tyler Weaver - Desc : Creates a parameter descriptor message used to describe parameters - Title : parameter_descriptor_builder.cpp - Project : moveit_servo -*/ - -#include - -namespace moveit_servo -{ -ParameterDescriptorBuilder & ParameterDescriptorBuilder::type(uint8_t type) -{ - msg_.type = type; - return *this; -} - -ParameterDescriptorBuilder & ParameterDescriptorBuilder::description( - const std::string & description) -{ - msg_.description = description; - return *this; -} - -ParameterDescriptorBuilder & ParameterDescriptorBuilder::additionalConstraints( - const std::string & additional_constraints) -{ - msg_.additional_constraints = additional_constraints; - return *this; -} - -ParameterDescriptorBuilder & ParameterDescriptorBuilder::readOnly(bool read_only) -{ - msg_.read_only = read_only; - return *this; -} - -// In rolling (not in Foxy) -ParameterDescriptorBuilder & ParameterDescriptorBuilder::dynamicTyping(bool dynamic_typing) -{ - msg_.dynamic_typing = dynamic_typing; - return *this; -} - -ParameterDescriptorBuilder & -ParameterDescriptorBuilder::floatingPointRange( - double from_value /*= std::numeric_limits::min()*/, - double to_value /*= std::numeric_limits::max()*/, - double step /*= 0*/) -{ - msg_.floating_point_range.resize(1); - msg_.floating_point_range[0].from_value = from_value; - msg_.floating_point_range[0].to_value = to_value; - msg_.floating_point_range[0].step = step; - return *this; -} - -ParameterDescriptorBuilder & -ParameterDescriptorBuilder::integerRange( - int64_t from_value /*= std::numeric_limits::min()*/, - int64_t to_value /*= std::numeric_limits::max()*/, - int64_t step /*= 0*/) -{ - msg_.integer_range.resize(1); - msg_.integer_range[0].from_value = from_value; - msg_.integer_range[0].to_value = to_value; - msg_.integer_range[0].step = step; - return *this; -} - -} // namespace moveit_servo diff --git a/urc_manipulation/src/pose_tracking.cpp b/urc_manipulation/src/pose_tracking.cpp deleted file mode 100644 index 5a109fda..00000000 --- a/urc_manipulation/src/pose_tracking.cpp +++ /dev/null @@ -1,420 +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 - -#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, const ServoParameters::SharedConstPtr & servo_parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr & planning_scene_monitor) -: node_(node) - , servo_parameters_(servo_parameters) - , 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_); - - // Use the C++ interface that Servo provides - servo_ = std::make_unique(node_, servo_parameters_, planning_scene_monitor_); - servo_->start(); - - // 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_->getParameters()->cartesian_command_in_topic, rclcpp::SystemDefaultsQoS()); -} - -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()) { - RCLCPP_WARN_STREAM_THROTTLE( - LOGGER, - *node_->get_clock(), LOG_THROTTLE_PERIOD, "Target control rate was missed"); - } - } - - doPostMotionReset(); - return PoseTrackingStatusCode::SUCCESS; -} - -void PoseTracking::readROSParams() -{ - const std::string ns = "moveit_servo"; - - declareOrGetParam(planning_frame_, ns + ".planning_frame", node_, LOGGER); - declareOrGetParam(move_group_name_, ns + ".move_group_name", node_, LOGGER); - - if (!planning_scene_monitor_->getRobotModel()->hasJointModelGroup(move_group_name_)) { - RCLCPP_ERROR_STREAM( - LOGGER, - "Unable to find the specified joint model group: " << move_group_name_); - } - - double publish_period; - declareOrGetParam(publish_period, ns + ".publish_period", node_, LOGGER); - - x_pid_config_.dt = publish_period; - y_pid_config_.dt = publish_period; - z_pid_config_.dt = publish_period; - angular_pid_config_.dt = publish_period; - - double windup_limit; - declareOrGetParam(windup_limit, ns + ".windup_limit", node_, LOGGER); - x_pid_config_.windup_limit = windup_limit; - y_pid_config_.windup_limit = windup_limit; - z_pid_config_.windup_limit = windup_limit; - angular_pid_config_.windup_limit = windup_limit; - - declareOrGetParam(x_pid_config_.k_p, ns + ".x_proportional_gain", node_, LOGGER); - declareOrGetParam(x_pid_config_.k_p, ns + ".x_proportional_gain", node_, LOGGER); - declareOrGetParam(y_pid_config_.k_p, ns + ".y_proportional_gain", node_, LOGGER); - declareOrGetParam(z_pid_config_.k_p, ns + ".z_proportional_gain", node_, LOGGER); - declareOrGetParam(x_pid_config_.k_i, ns + ".x_integral_gain", node_, LOGGER); - declareOrGetParam(y_pid_config_.k_i, ns + ".y_integral_gain", node_, LOGGER); - declareOrGetParam(z_pid_config_.k_i, ns + ".z_integral_gain", node_, LOGGER); - declareOrGetParam(x_pid_config_.k_d, ns + ".x_derivative_gain", node_, LOGGER); - declareOrGetParam(y_pid_config_.k_d, ns + ".y_derivative_gain", node_, LOGGER); - declareOrGetParam(z_pid_config_.k_d, ns + ".z_derivative_gain", node_, LOGGER); - - declareOrGetParam(angular_pid_config_.k_p, ns + ".angular_proportional_gain", node_, LOGGER); - declareOrGetParam(angular_pid_config_.k_i, ns + ".angular_integral_gain", node_, LOGGER); - declareOrGetParam(angular_pid_config_.k_d, ns + ".angular_derivative_gain", node_, LOGGER); -} - -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/urc_manipulation/src/servo.cpp b/urc_manipulation/src/servo.cpp deleted file mode 100644 index de51e294..00000000 --- a/urc_manipulation/src/servo.cpp +++ /dev/null @@ -1,118 +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.cpp - * Project : moveit_servo - * Created : 3/9/2017 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - */ - -#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 ServoParameters::SharedConstPtr & parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr & planning_scene_monitor) -: planning_scene_monitor_{planning_scene_monitor} - , parameters_{parameters} - , servo_calcs_{node, parameters, planning_scene_monitor_} - , collision_checker_{node, parameters, planning_scene_monitor_} -{ -} - -void Servo::start() -{ - if (!planning_scene_monitor_->getStateMonitor()->waitForCompleteState( - parameters_->move_group_name, - ROBOT_STATE_WAIT_TIME)) - { - RCLCPP_ERROR(LOGGER, "Timeout waiting for current state"); - return; - } - - setPaused(false); - - // Crunch the numbers in this timer - servo_calcs_.start(); - - // Check collisions in this timer - if (parameters_->check_collisions) { - collision_checker_.start(); - } -} - -Servo::~Servo() -{ - setPaused(true); -} - -void Servo::setPaused(bool paused) -{ - servo_calcs_.setPaused(paused); - collision_checker_.setPaused(paused); -} - -bool Servo::getCommandFrameTransform(Eigen::Isometry3d & transform) -{ - return servo_calcs_.getCommandFrameTransform(transform); -} - -bool Servo::getCommandFrameTransform(geometry_msgs::msg::TransformStamped & transform) -{ - return servo_calcs_.getCommandFrameTransform(transform); -} - -bool Servo::getEEFrameTransform(Eigen::Isometry3d & transform) -{ - return servo_calcs_.getEEFrameTransform(transform); -} - -bool Servo::getEEFrameTransform(geometry_msgs::msg::TransformStamped & transform) -{ - return servo_calcs_.getEEFrameTransform(transform); -} - -const ServoParameters::SharedConstPtr & Servo::getParameters() const -{ - return parameters_; -} - -} // namespace moveit_servo diff --git a/urc_manipulation/src/servo_calcs.cpp b/urc_manipulation/src/servo_calcs.cpp deleted file mode 100644 index 4b99af06..00000000 --- a/urc_manipulation/src/servo_calcs.cpp +++ /dev/null @@ -1,1223 +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 // TODO(adamp): create an issue about this -#include -#include -#include - -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 -} // namespace - -// Constructor for the class that handles servoing calculations -ServoCalcs::ServoCalcs( - const rclcpp::Node::SharedPtr & node, - const std::shared_ptr & parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr & planning_scene_monitor) -: node_(node) - , parameters_(parameters) - , planning_scene_monitor_(planning_scene_monitor) - , stop_requested_(true) - , done_stopping_(false) - , paused_(false) - , robot_link_command_frame_(parameters->robot_link_command_frame) - , smoothing_loader_("moveit_core", "online_signal_smoothing::SmoothingBaseClass") -{ - // Register callback for changes in robot_link_command_frame - bool callback_success = parameters_->registerSetParameterCallback( - parameters->ns + ".robot_link_command_frame", - [this](const rclcpp::Parameter & parameter) { - return robotLinkCommandFrameCallback(parameter); - }); - if (!callback_success) { - throw std::runtime_error("Failed to register setParameterCallback"); - } - - // MoveIt Setup - current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); - joint_model_group_ = current_state_->getJointModelGroup(parameters_->move_group_name); - if (joint_model_group_ == nullptr) { - RCLCPP_ERROR_STREAM( - LOGGER, - "Invalid move group name: `" << parameters_->move_group_name << "`"); - throw std::runtime_error("Invalid move group name"); - } - - // Subscribe to command topics - twist_stamped_sub_ = node_->create_subscription( - parameters_->cartesian_command_in_topic, rclcpp::SystemDefaultsQoS(), - [this](const geometry_msgs::msg::TwistStamped::ConstSharedPtr & msg) { - return twistStampedCB(msg); - }); - - joint_cmd_sub_ = node_->create_subscription( - parameters_->joint_command_in_topic, rclcpp::SystemDefaultsQoS(), - [this](const control_msgs::msg::JointJog::ConstSharedPtr & msg) {return jointCmdCB(msg);}); - - // ROS Server for allowing drift in some dimensions - drift_dimensions_server_ = node_->create_service( - "~/change_drift_dimensions", - [this](const std::shared_ptr & req, - const std::shared_ptr & res) { - return changeDriftDimensions(req, res); - }); - - // ROS Server for changing the control dimensions - control_dimensions_server_ = node_->create_service( - "~/change_control_dimensions", - [this](const std::shared_ptr & req, - const std::shared_ptr & res) { - return changeControlDimensions(req, res); - }); - - // ROS Server to reset the status, e.g. so the arm can move again after a collision - reset_servo_status_ = node_->create_service( - "~/reset_servo_status", - [this](const std::shared_ptr & req, - const std::shared_ptr & res) { - return resetServoStatus(req, res); - }); - - // 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 (parameters_->command_out_type == "trajectory_msgs/JointTrajectory") { - trajectory_outgoing_cmd_pub_ = node_->create_publisher( - parameters_->command_out_topic, rclcpp::SystemDefaultsQoS()); - } else if (parameters_->command_out_type == "std_msgs/Float64MultiArray") { - multiarray_outgoing_cmd_pub_ = node_->create_publisher( - parameters_->command_out_topic, rclcpp::SystemDefaultsQoS()); - } - - // Publish status - status_pub_ = node_->create_publisher( - parameters_->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_.createSharedInstance(parameters_->smoothing_filter_plugin_name); - } catch (pluginlib::PluginlibException & ex) { - RCLCPP_ERROR( - LOGGER, "Exception while loading the smoothing plugin '%s': '%s'", - parameters_->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_) { - use_inv_jacobian_ = true; - 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_)) { - use_inv_jacobian_ = true; - 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 = parameters_->planning_frame; - initial_joint_trajectory->joint_names = internal_joint_state_.name; - trajectory_msgs::msg::JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(parameters_->publish_period); - if (parameters_->publish_joint_positions) { - planning_scene_monitor_->getStateMonitor()->getCurrentState()->copyJointGroupPositions( - joint_model_group_, - point.positions); - } - if (parameters_->publish_joint_velocities) { - std::vector velocity(num_joints_); - point.velocities = velocity; - } - if (parameters_->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(); - tf_moveit_to_ee_frame_ = - current_state_->getGlobalLinkTransform(parameters_->planning_frame).inverse() * - current_state_->getGlobalLinkTransform(parameters_->ee_frame_name); - tf_moveit_to_robot_cmd_frame_ = - current_state_->getGlobalLinkTransform(parameters_->planning_frame).inverse() * - current_state_->getGlobalLinkTransform(robot_link_command_frame_); - if (!use_inv_jacobian_) { - ik_base_to_tip_frame_ = - current_state_->getGlobalLinkTransform(ik_solver_->getBaseFrame()).inverse() * - current_state_->getGlobalLinkTransform(ik_solver_->getTipFrame()); - } - - stop_requested_ = false; - thread_ = std::thread([this] {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::mainCalcLoop() -{ - rclcpp::WallRate rate(1.0 / parameters_->publish_period); - - while (rclcpp::ok() && !stop_requested_) { - // lock the input state mutex - std::unique_lock main_loop_lock(main_loop_mutex_); - - // low latency mode -- begin calculations as soon as a new command is received. - if (parameters_->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() > parameters_->publish_period) { - rclcpp::Clock & clock = *node_->get_clock(); - RCLCPP_WARN_STREAM_THROTTLE( - LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "run_duration: " << run_duration.seconds() << " (" << parameters_->publish_period - << ")"); - } - - // normal mode, unlock input mutex and wait for the period of the loop - if (!parameters_->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 - updateJoints(); - - // Update from latest state - current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); - - 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(parameters_->incoming_command_timeout)); - joint_command_is_stale_ = ((node_->now() - latest_joint_command_stamp_) >= - rclcpp::Duration::from_seconds(parameters_->incoming_command_timeout)); - - have_nonzero_twist_stamped_ = latest_twist_cmd_is_nonzero_; - have_nonzero_joint_command_ = latest_joint_cmd_is_nonzero_; - - // 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(parameters_->planning_frame).inverse() * - current_state_->getGlobalLinkTransform(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(parameters_->planning_frame).inverse() * - current_state_->getGlobalLinkTransform(parameters_->ee_frame_name); - - if (!use_inv_jacobian_) { - ik_base_to_tip_frame_ = - current_state_->getGlobalLinkTransform(ik_solver_->getBaseFrame()).inverse() * - current_state_->getGlobalLinkTransform(ik_solver_->getTipFrame()); - } - - have_nonzero_command_ = have_nonzero_twist_stamped_ || have_nonzero_joint_command_; - - // Don't end this function without updating the filters - updated_filters_ = false; - - // If paused or while 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_ || paused_) { - resetLowPassFilters(original_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, and not paused. - // 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 and nonzero - if (have_nonzero_twist_stamped_ && !twist_command_is_stale_) { - if (!cartesianServoCalcs(twist_stamped_cmd_, *joint_trajectory)) { - resetLowPassFilters(original_joint_state_); - return; - } - } else if (have_nonzero_joint_command_ && !joint_command_is_stale_) { - if (!jointServoCalcs(joint_servo_cmd_, *joint_trajectory)) { - resetLowPassFilters(original_joint_state_); - return; - } - } else { - // Joint trajectory is not populated with anything, so set it to the last positions and 0 velocity - *joint_trajectory = *last_sent_command_; - for (auto & point : joint_trajectory->points) { - point.velocities.assign(point.velocities.size(), 0); - } - } - - // Print a warning to the user if both are stale - if (twist_command_is_stale_ && joint_command_is_stale_) { - filteredHalt(*joint_trajectory); - } else { - done_stopping_ = false; - } - - // Skip the servoing publication if all inputs have been zero for several cycles in a row. - // num_outgoing_halt_msgs_to_publish == 0 signifies that we should keep republishing forever. - if (!have_nonzero_command_ && done_stopping_ && - (parameters_->num_outgoing_halt_msgs_to_publish != 0) && - (zero_velocity_count_ > parameters_->num_outgoing_halt_msgs_to_publish)) - { - ok_to_publish_ = false; - rclcpp::Clock & clock = *node_->get_clock(); - RCLCPP_DEBUG_STREAM_THROTTLE( - LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "All-zero command. Doing nothing."); - } - // Skip servoing publication if both types of commands are stale. - else if (twist_command_is_stale_ && joint_command_is_stale_) { - ok_to_publish_ = false; - rclcpp::Clock & clock = *node_->get_clock(); - RCLCPP_DEBUG_STREAM_THROTTLE( - LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "Skipping publishing because incoming commands are stale."); - } else { - ok_to_publish_ = true; - } - - // Store last zero-velocity message flag to prevent superfluous warnings. - // Cartesian and joint commands must both be zero. - if (!have_nonzero_command_ && done_stopping_) { - // Avoid overflow - if (zero_velocity_count_ < std::numeric_limits::max()) { - ++zero_velocity_count_; - } - } else { - zero_velocity_count_ = 0; - } - - if (ok_to_publish_ && !paused_) { - // Clear out position commands if user did not request them (can cause interpolation issues) - if (!parameters_->publish_joint_positions) { - joint_trajectory->points[0].positions.clear(); - } - // Likewise for velocity and acceleration - if (!parameters_->publish_joint_velocities) { - joint_trajectory->points[0].velocities.clear(); - } - if (!parameters_->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 (parameters_->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 (parameters_->command_out_type == "std_msgs/Float64MultiArray") { - auto joints = std::make_unique(); - if (parameters_->publish_joint_positions && !joint_trajectory->points.empty()) { - joints->data = joint_trajectory->points[0].positions; - } else if (parameters_->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(original_joint_state_); - } -} - -rcl_interfaces::msg::SetParametersResult ServoCalcs::robotLinkCommandFrameCallback( - const rclcpp::Parameter & parameter) -{ - const std::lock_guard lock(main_loop_mutex_); - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - robot_link_command_frame_ = parameter.as_string(); - RCLCPP_INFO_STREAM(LOGGER, "robot_link_command_frame changed to: " + robot_link_command_frame_); - return result; -} - -// 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; - } - - // Set uncontrolled dimensions to 0 in command frame - enforceControlDimensions(cmd); - - // Transform the command to the MoveGroup planning frame - if (cmd.header.frame_id != parameters_->planning_frame) { - 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); - - // If the incoming frame is empty or is the command frame, we use the previously calculated tf - if (cmd.header.frame_id.empty() || cmd.header.frame_id == robot_link_command_frame_) { - translation_vector = tf_moveit_to_robot_cmd_frame_.linear() * translation_vector; - angular_vector = tf_moveit_to_robot_cmd_frame_.linear() * angular_vector; - } else if (cmd.header.frame_id == parameters_->ee_frame_name) { - // If the frame is the EE frame, we already have that transform as well - translation_vector = tf_moveit_to_ee_frame_.linear() * translation_vector; - angular_vector = tf_moveit_to_ee_frame_.linear() * angular_vector; - } else { - // We solve (planning_frame -> base -> cmd.header.frame_id) - // by computing (base->planning_frame)^-1 * (base->cmd.header.frame_id) - const auto tf_moveit_to_incoming_cmd_frame = - current_state_->getGlobalLinkTransform(parameters_->planning_frame).inverse() * - current_state_->getGlobalLinkTransform(cmd.header.frame_id); - translation_vector = tf_moveit_to_incoming_cmd_frame.linear() * translation_vector; - angular_vector = tf_moveit_to_incoming_cmd_frame.linear() * angular_vector; - } - - // Put these components back into a TwistStamped - cmd.header.frame_id = parameters_->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); - } - - Eigen::VectorXd delta_x = scaleCartesianCommand(cmd); - - Eigen::MatrixXd jacobian = current_state_->getJacobian(joint_model_group_); - - removeDriftDimensions(jacobian, delta_x); - - 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 (!use_inv_jacobian_) { - // 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); - - // Poses passed to IK solvers are assumed to be in some tip link (usually EE) reference frame - // First, find the new tip link position without newly applied rotation - - auto tf_no_new_rot = tf_pos_delta * ik_base_to_tip_frame_; - // 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 - auto tf_translation = tf_no_new_rot.translation(); - auto 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); - auto 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 - auto tf = tf_pos_translation * tf_rot_delta * tf_neg_translation * tf_no_new_rot; - geometry_msgs::msg::Pose next_pose = tf2::toMsg(tf); - - // 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, parameters_->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; - } - - delta_theta_ *= velocityScalingFactorForSingularity( - joint_model_group_, delta_x, svd, pseudo_inverse, - parameters_->hard_stop_singularity_threshold, - parameters_->lower_singularity_threshold, - parameters_->leaving_singularity_threshold_multiplier, - *node_->get_clock(), current_state_, 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. - - // Set internal joint state from original - internal_joint_state_ = original_joint_state_; - - // 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 thru joints and update them, calculate velocities, and filter - if (!applyJointUpdate(delta_theta, internal_joint_state_)) { - return false; - } - - // Mark the lowpass filters as updated for this cycle - updated_filters_ = true; - - // Enforce SRDF velocity limits - enforceVelocityLimits( - joint_model_group_, parameters_->publish_period, internal_joint_state_, - parameters_->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_); - if (!joints_to_halt.empty()) { - status_ = StatusCode::JOINT_BOUND; - if ((servo_type == ServoType::JOINT_SPACE && !parameters_->halt_all_joints_in_joint_mode) || - (servo_type == ServoType::CARTESIAN_SPACE && !parameters_->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); - - // Modify the output message if we are using gazebo - if (parameters_->use_gazebo) { - insertRedundantPointsIntoTrajectory(joint_trajectory, gazebo_redundant_message_count_); - } - - return true; -} - -bool ServoCalcs::applyJointUpdate( - const Eigen::ArrayXd & delta_theta, - sensor_msgs::msg::JointState & joint_state) -{ - // All the sizes must match - if (joint_state.position.size() != static_cast(delta_theta.size()) || - joint_state.velocity.size() != joint_state.position.size()) - { - rclcpp::Clock & clock = *node_->get_clock(); - RCLCPP_ERROR_STREAM_THROTTLE( - LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - "Lengths of output and increments do not match."); - return false; - } - - for (std::size_t i = 0; i < joint_state.position.size(); ++i) { - // Increment joint - joint_state.position[i] += delta_theta[i]; - } - - smoother_->doSmoothing(joint_state.position); - - for (std::size_t i = 0; i < joint_state.position.size(); ++i) { - // Calculate joint velocity - joint_state.velocity[i] = - (joint_state.position.at(i) - original_joint_state_.position.at(i)) / - parameters_->publish_period; - } - - return true; -} - -// Spam several redundant points into the trajectory. The first few may be skipped if the -// time stamp is in the past when it reaches the client. Needed for gazebo simulation. -// Start from 1 because the first point's timestamp is already 1*parameters_->publish_period -void ServoCalcs::insertRedundantPointsIntoTrajectory( - trajectory_msgs::msg::JointTrajectory & joint_trajectory, - int count) const -{ - if (count < 2) { - return; - } - joint_trajectory.points.resize(count); - auto point = joint_trajectory.points[0]; - // Start from 1 because we already have the first point. End at count+1 so (total #) == count - for (int i = 1; i < count; ++i) { - point.time_from_start = rclcpp::Duration::from_seconds((i + 1) * parameters_->publish_period); - joint_trajectory.points[i] = point; - } -} - -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 = parameters_->planning_frame; - joint_trajectory.joint_names = joint_state.name; - - trajectory_msgs::msg::JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(parameters_->publish_period); - if (parameters_->publish_joint_positions) { - point.positions = joint_state.position; - } - if (parameters_->publish_joint_velocities) { - point.velocities = joint_state.velocity; - } - if (parameters_->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); -} - -std::vector -ServoCalcs::enforcePositionLimits(sensor_msgs::msg::JointState & joint_state) const -{ - // 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, -parameters_->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 + parameters_->joint_limit_margin))) || - (joint_state.velocity.at(joint_idx) > 0 && - (joint_angle > (limits[0].max_position - parameters_->joint_limit_margin)))) - { - joints_to_halt.push_back(joint); - } - } - } - } - if (!joints_to_halt.empty()) { - std::ostringstream joints_names; - std::transform( - joints_to_halt.cbegin(), std::prev(joints_to_halt.cend()), - std::ostream_iterator(joints_names, ", "), - [](const auto & joint) {return joint->getName();}); - joints_names << joints_to_halt.back()->getName(); - RCLCPP_WARN_STREAM_THROTTLE( - LOGGER, *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD, - node_->get_name() - << " " << joints_names.str() << " close to a position limit. Halting."); - } - return joints_to_halt; -} - -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(); - - // Deceleration algorithm: - // Set positions to original_joint_state_ - // Filter - // Calculate velocities - // Check if velocities are close to zero. Round to zero, if so. - // Set done_stopping_ flag - assert(original_joint_state_.position.size() >= num_joints_); - joint_trajectory.points[0].positions = original_joint_state_.position; - smoother_->doSmoothing(joint_trajectory.points[0].positions); - done_stopping_ = true; - if (parameters_->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) - original_joint_state_.position.at(i)) / - parameters_->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 (parameters_->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) - original_joint_state_.velocity.at(i)) / - parameters_->publish_period; - } - } - - joint_trajectory.points[0].time_from_start = rclcpp::Duration::from_seconds( - parameters_->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) = original_joint_state_.position.at(joint_index); - joint_state.velocity.at(joint_index) = 0.0; - } - } -} - -void ServoCalcs::updateJoints() -{ - // 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); - - // Cache the original joints in case they need to be reset - original_joint_state_ = internal_joint_state_; -} - -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 (parameters_->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; - } - } - - 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 (parameters_->command_in_type == "unitless") { - result[0] = parameters_->linear_scale * parameters_->publish_period * command.twist.linear.x; - result[1] = parameters_->linear_scale * parameters_->publish_period * command.twist.linear.y; - result[2] = parameters_->linear_scale * parameters_->publish_period * command.twist.linear.z; - result[3] = parameters_->rotational_scale * parameters_->publish_period * - command.twist.angular.x; - result[4] = parameters_->rotational_scale * parameters_->publish_period * - command.twist.angular.y; - result[5] = parameters_->rotational_scale * parameters_->publish_period * - command.twist.angular.z; - } - // Otherwise, commands are in m/s and rad/s - else if (parameters_->command_in_type == "speed_units") { - result[0] = command.twist.linear.x * parameters_->publish_period; - result[1] = command.twist.linear.y * parameters_->publish_period; - result[2] = command.twist.linear.z * parameters_->publish_period; - result[3] = command.twist.angular.x * parameters_->publish_period; - result[4] = command.twist.angular.y * parameters_->publish_period; - result[5] = command.twist.angular.z * parameters_->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 (parameters_->command_in_type == "unitless") { - result[c] = command.velocities[m] * parameters_->joint_scale * parameters_->publish_period; - } - // Otherwise, commands are in m/s and rad/s - else if (parameters_->command_in_type == "speed_units") { - result[c] = command.velocities[m] * parameters_->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; -} - -void ServoCalcs::removeDimension( - Eigen::MatrixXd & jacobian, Eigen::VectorXd & delta_x, - unsigned int row_to_remove) const -{ - unsigned int num_rows = jacobian.rows() - 1; - unsigned int num_cols = jacobian.cols(); - - if (row_to_remove < num_rows) { - jacobian.block(row_to_remove, 0, num_rows - row_to_remove, num_cols) = - jacobian.block(row_to_remove + 1, 0, num_rows - row_to_remove, num_cols); - delta_x.segment(row_to_remove, num_rows - row_to_remove) = - delta_x.segment(row_to_remove + 1, num_rows - row_to_remove); - } - jacobian.conservativeResize(num_rows, num_cols); - delta_x.conservativeResize(num_rows); -} - -void ServoCalcs::removeDriftDimensions(Eigen::MatrixXd & matrix, Eigen::VectorXd & delta_x) -{ - // May allow some dimensions to drift, based on drift_dimensions - // i.e. take advantage of task redundancy. - // Remove the Jacobian rows corresponding to True in the vector drift_dimensions - // Work backwards through the 6-vector so indices don't get out of order - for (auto dimension = matrix.rows() - 1; dimension >= 0; --dimension) { - if (drift_dimensions_[dimension] && matrix.rows() > 1) { - removeDimension(matrix, delta_x, dimension); - } - } -} - -void ServoCalcs::enforceControlDimensions(geometry_msgs::msg::TwistStamped & command) -{ - // Can't loop through the message, so check them all - if (!control_dimensions_[0]) { - command.twist.linear.x = 0; - } - if (!control_dimensions_[1]) { - command.twist.linear.y = 0; - } - if (!control_dimensions_[2]) { - command.twist.linear.z = 0; - } - if (!control_dimensions_[3]) { - command.twist.angular.x = 0; - } - if (!control_dimensions_[4]) { - command.twist.angular.y = 0; - } - if (!control_dimensions_[5]) { - command.twist.angular.z = 0; - } -} - -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_, parameters_->planning_frame, - 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_, parameters_->planning_frame, - parameters_->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; - latest_twist_cmd_is_nonzero_ = isNonZero(*latest_twist_stamped_); - - 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; - latest_joint_cmd_is_nonzero_ = isNonZero(*latest_joint_cmd_); - - 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; -} - -void ServoCalcs::changeDriftDimensions( - const std::shared_ptr & req, - const std::shared_ptr & res) -{ - drift_dimensions_[0] = req->drift_x_translation; - drift_dimensions_[1] = req->drift_y_translation; - drift_dimensions_[2] = req->drift_z_translation; - drift_dimensions_[3] = req->drift_x_rotation; - drift_dimensions_[4] = req->drift_y_rotation; - drift_dimensions_[5] = req->drift_z_rotation; - - res->success = true; -} - -void ServoCalcs::changeControlDimensions( - const std::shared_ptr & req, - const std::shared_ptr & res) -{ - control_dimensions_[0] = req->control_x_translation; - control_dimensions_[1] = req->control_y_translation; - control_dimensions_[2] = req->control_z_translation; - control_dimensions_[3] = req->control_x_rotation; - control_dimensions_[4] = req->control_y_rotation; - control_dimensions_[5] = req->control_z_rotation; - - res->success = true; -} - -bool ServoCalcs::resetServoStatus( - const std::shared_ptr & /*req*/, - const std::shared_ptr & /*res*/) -{ - status_ = StatusCode::NO_WARNING; - return true; -} - -void ServoCalcs::setPaused(bool paused) -{ - paused_ = paused; -} - -} // namespace moveit_servo diff --git a/urc_manipulation/src/servo_node.cpp b/urc_manipulation/src/servo_node.cpp deleted file mode 100644 index 06e987e8..00000000 --- a/urc_manipulation/src/servo_node.cpp +++ /dev/null @@ -1,153 +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 -#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); - }); - pause_servo_service_ = node_->create_service( - "~/pause_servo", - [this](const std::shared_ptr & request, - const std::shared_ptr & response) { - return pauseCB(request, response); - }); - unpause_servo_service_ = node_->create_service( - "~/unpause_servo", [this](const std::shared_ptr & request, - const std::shared_ptr & response) { - return unpauseCB(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 servo_parameters = moveit_servo::ServoParameters::makeServoParameters(node_); - if (servo_parameters == nullptr) { - RCLCPP_ERROR(LOGGER, "Failed to load the servo parameters"); - throw std::runtime_error("Failed to load the 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_, servo_parameters, planning_scene_monitor_); -} - -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_->setPaused(true); - response->success = true; -} - -void ServoNode::pauseCB( - const std::shared_ptr & /* unused */, - const std::shared_ptr & response) -{ - servo_->setPaused(true); - response->success = true; -} - -void ServoNode::unpauseCB( - const std::shared_ptr & /* unused */, - const std::shared_ptr & response) -{ - servo_->setPaused(false); - response->success = true; -} - -} // namespace moveit_servo - -// Register the component with class_loader -#include -RCLCPP_COMPONENTS_REGISTER_NODE(moveit_servo::ServoNode) diff --git a/urc_manipulation/src/servo_node_main.cpp b/urc_manipulation/src/servo_node_main.cpp deleted file mode 100644 index 27fb3d53..00000000 --- a/urc_manipulation/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/urc_manipulation/src/servo_parameters.cpp b/urc_manipulation/src/servo_parameters.cpp deleted file mode 100644 index b670b3b2..00000000 --- a/urc_manipulation/src/servo_parameters.cpp +++ /dev/null @@ -1,544 +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 : Declares, loads, and checks ServoParameters for Servo - Title : servo_parameters.cpp - Project : moveit_servo - Created : 07/02/2020 -*/ - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace moveit_servo -{ -namespace -{ -const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo_parameters"); -} - -rcl_interfaces::msg::SetParametersResult -ServoParameters::CallbackHandler::setParametersCallback( - const std::vector & parameters) -{ - const std::lock_guard guard(mutex_); - auto result = rcl_interfaces::msg::SetParametersResult(); - result.successful = true; - - for (const auto & parameter : parameters) { - auto search = set_parameter_callbacks_.find(parameter.get_name()); - if (search != set_parameter_callbacks_.end()) { - RCLCPP_INFO_STREAM( - LOGGER, "setParametersCallback - " - << parameter.get_name() << "<" << parameter.get_type_name() - << ">: " << rclcpp::to_string(parameter.get_parameter_value())); - for (const auto & callback : search->second) { - result = callback(parameter); - - if (!result.successful) { - // Handle automatic parameter set failure - return result; - } - } - } - } - - return result; -} - -void ServoParameters::declare( - const std::string & ns, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters) -{ - using rclcpp::ParameterValue; - using rclcpp::ParameterType::PARAMETER_BOOL; - using rclcpp::ParameterType::PARAMETER_DOUBLE; - using rclcpp::ParameterType::PARAMETER_INTEGER; - using rclcpp::ParameterType::PARAMETER_STRING; - auto parameters = ServoParameters{}; - - // ROS Parameters - node_parameters->declare_parameter( - ns + ".use_gazebo", ParameterValue{parameters.use_gazebo}, - ParameterDescriptorBuilder{} - .type(PARAMETER_BOOL) - .description("Whether the robot is started in a Gazebo simulation environment")); - node_parameters->declare_parameter( - ns + ".status_topic", ParameterValue{parameters.status_topic}, - ParameterDescriptorBuilder{}.type(PARAMETER_STRING).description("Publish status to this topic")); - // Properties of incoming commands - node_parameters->declare_parameter( - ns + ".cartesian_command_in_topic", ParameterValue{parameters.cartesian_command_in_topic}, - ParameterDescriptorBuilder{}.type(PARAMETER_STRING).description( - "Topic for incoming Cartesian twist commands")); - node_parameters->declare_parameter( - ns + ".joint_command_in_topic", ParameterValue{parameters.joint_command_in_topic}, - ParameterDescriptorBuilder{}.type(PARAMETER_STRING).description( - "Topic for incoming joint angle commands")); - node_parameters->declare_parameter( - ns + ".robot_link_command_frame", ParameterValue{parameters.robot_link_command_frame}, - ParameterDescriptorBuilder{} - .type(PARAMETER_STRING) - .description( - "Commands must be given in the frame of a robot link. Usually either the base or end effector")); - node_parameters->declare_parameter( - ns + ".command_in_type", ParameterValue{parameters.command_in_type}, - ParameterDescriptorBuilder{} - .type(PARAMETER_STRING) - .description( - "\"unitless\"> in the range [-1:1], as if from joystick. " - "\"speed_units\"> cmds are in m/s and rad/s")); - node_parameters->declare_parameter( - ns + ".scale.linear", ParameterValue{parameters.linear_scale}, - ParameterDescriptorBuilder{} - .type(PARAMETER_DOUBLE) - .description( - "Max linear velocity. Unit is [m/s]. " - "Only used for Cartesian commands.")); - node_parameters->declare_parameter( - ns + ".scale.rotational", ParameterValue{parameters.rotational_scale}, - ParameterDescriptorBuilder{} - .type(PARAMETER_DOUBLE) - .description( - "Max angular velocity. Unit is [rad/s]. " - "Only used for Cartesian commands.")); - node_parameters->declare_parameter( - ns + ".scale.joint", ParameterValue{parameters.joint_scale}, - ParameterDescriptorBuilder{} - .type(PARAMETER_DOUBLE) - .description( - "Max joint angular/linear velocity. Only used for joint " - "commands on joint_command_in_topic.")); - - // Properties of Servo calculations - node_parameters->declare_parameter( - ns + ".override_velocity_scaling_factor", - ParameterValue{parameters.override_velocity_scaling_factor}, - ParameterDescriptorBuilder{} - .type(PARAMETER_DOUBLE) - .description( - "Override constant scalar of how fast the robot should jog." - "Valid values are between 0-1.0")); - - // Properties of outgoing commands - node_parameters->declare_parameter( - ns + ".command_out_topic", ParameterValue{parameters.command_out_topic}, - ParameterDescriptorBuilder{}.type(PARAMETER_STRING).description( - "Publish outgoing commands here")); - node_parameters->declare_parameter( - ns + ".publish_period", ParameterValue{parameters.publish_period}, - ParameterDescriptorBuilder{}.type(PARAMETER_DOUBLE).description( - "1/Nominal publish rate [seconds]")); - node_parameters->declare_parameter( - ns + ".command_out_type", ParameterValue{parameters.command_out_type}, - ParameterDescriptorBuilder{} - .type(PARAMETER_STRING) - .description( - "What type of topic does your robot driver expect? Currently supported are " - "std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory")); - node_parameters->declare_parameter( - ns + ".publish_joint_positions", ParameterValue{parameters.publish_joint_positions}, - ParameterDescriptorBuilder{} - .type(PARAMETER_BOOL) - .description( - "What to publish? Can save some bandwidth as most robots only require positions or velocities")); - node_parameters->declare_parameter( - ns + ".publish_joint_velocities", ParameterValue{parameters.publish_joint_velocities}, - ParameterDescriptorBuilder{} - .type(PARAMETER_BOOL) - .description( - "What to publish? Can save some bandwidth as most robots only require positions or velocities")); - node_parameters->declare_parameter( - ns + ".publish_joint_accelerations", ParameterValue{parameters.publish_joint_accelerations}, - ParameterDescriptorBuilder{} - .type(PARAMETER_BOOL) - .description( - "What to publish? Can save some bandwidth as most robots only require positions or velocities")); - node_parameters->declare_parameter( - ns + ".low_latency_mode", ParameterValue{parameters.low_latency_mode}, - ParameterDescriptorBuilder{}.type(PARAMETER_BOOL).description("Low latency mode")); - - // Incoming Joint State properties - node_parameters->declare_parameter( - ns + ".joint_topic", ParameterValue{parameters.joint_topic}, - ParameterDescriptorBuilder{}.type(PARAMETER_STRING).description("Joint topic")); - node_parameters->declare_parameter( - ns + ".smoothing_filter_plugin_name", ParameterValue{parameters.smoothing_filter_plugin_name}, - ParameterDescriptorBuilder{}.type(PARAMETER_STRING).description( - "Plugins for smoothing outgoing commands")); - - // MoveIt properties - node_parameters->declare_parameter( - ns + ".move_group_name", ParameterValue{parameters.move_group_name}, - ParameterDescriptorBuilder{}.type(PARAMETER_STRING).description("Often 'manipulator' or 'arm'")); - node_parameters->declare_parameter( - ns + ".planning_frame", ParameterValue{parameters.planning_frame}, - ParameterDescriptorBuilder{} - .type(PARAMETER_STRING) - .description("The MoveIt planning frame. Often 'base_link' or 'world'")); - node_parameters->declare_parameter( - ns + ".ee_frame_name", ParameterValue{parameters.ee_frame_name}, - ParameterDescriptorBuilder{} - .type(PARAMETER_STRING) - .description("The name of the end effector link, used to return the EE pose")); - node_parameters->declare_parameter( - ns + ".is_primary_planning_scene_monitor", - ParameterValue{parameters.is_primary_planning_scene_monitor}, - ParameterDescriptorBuilder{}.type(PARAMETER_BOOL).description( - "Is primary planning scene monitor")); - node_parameters->declare_parameter( - ns + ".monitored_planning_scene_topic", - ParameterValue{parameters.monitored_planning_scene_topic}, - ParameterDescriptorBuilder{}.type(PARAMETER_STRING).description( - "Monitored planning scene topic")); - - // Stopping behaviour - node_parameters->declare_parameter( - ns + ".incoming_command_timeout", ParameterValue{parameters.incoming_command_timeout}, - ParameterDescriptorBuilder{} - .type(PARAMETER_DOUBLE) - .description( - "Stop servoing if X seconds elapse without a new command. If 0, republish commands forever even " - "if the robot is stationary.Otherwise, specify num.to publish. Important because ROS may drop " - "some messages and we need the robot to halt reliably.")); - node_parameters->declare_parameter( - ns + ".num_outgoing_halt_msgs_to_publish", - ParameterValue{parameters.num_outgoing_halt_msgs_to_publish}, - ParameterDescriptorBuilder{}.type(PARAMETER_INTEGER).description( - "Num outgoing halt msgs to publish")); - node_parameters->declare_parameter( - ns + ".halt_all_joints_in_joint_mode", ParameterValue{parameters.halt_all_joints_in_joint_mode}, - ParameterDescriptorBuilder{}.type(PARAMETER_BOOL).description("Halt all joints in joint mode")); - node_parameters->declare_parameter( - ns + ".halt_all_joints_in_cartesian_mode", - ParameterValue{parameters.halt_all_joints_in_cartesian_mode}, - ParameterDescriptorBuilder{}.type(PARAMETER_BOOL).description( - "Halt all joints in cartesian mode")); - - // Configure handling of singularities and joint limits - node_parameters->declare_parameter( - ns + ".lower_singularity_threshold", ParameterValue{parameters.lower_singularity_threshold}, - ParameterDescriptorBuilder{} - .type(PARAMETER_DOUBLE) - .description("Start decelerating when the condition number hits this (close to singularity)")); - node_parameters->declare_parameter( - ns + ".hard_stop_singularity_threshold", - ParameterValue{parameters.hard_stop_singularity_threshold}, - ParameterDescriptorBuilder{}.type(PARAMETER_DOUBLE).description( - "Stop when the condition number hits this")); - node_parameters->declare_parameter( - ns + ".joint_limit_margin", ParameterValue{parameters.joint_limit_margin}, - ParameterDescriptorBuilder{} - .type(PARAMETER_DOUBLE) - .description( - "Added as a buffer to joint limits [radians]. If moving quickly, make this larger.")); - node_parameters->declare_parameter( - ns + ".leaving_singularity_threshold_multiplier", - ParameterValue{parameters.leaving_singularity_threshold_multiplier}, - ParameterDescriptorBuilder{} - .type(PARAMETER_DOUBLE) - .description( - "When 'lower_singularity_threshold' is triggered, but we are moving away from singularity, move " - "this many times faster than if we were moving further into singularity")); - - // Collision checking - node_parameters->declare_parameter( - ns + ".check_collisions", ParameterValue{parameters.check_collisions}, - ParameterDescriptorBuilder{}.type(PARAMETER_BOOL).description("Check collisions?")); - node_parameters->declare_parameter( - ns + ".collision_check_rate", ParameterValue(parameters.collision_check_rate), - ParameterDescriptorBuilder{} - .type(PARAMETER_DOUBLE) - .description( - "[Hz] Collision-checking can easily bog down a CPU if done too often. Collision checking begins " - "slowing down when nearer than a specified distance.")); - node_parameters->declare_parameter( - ns + ".self_collision_proximity_threshold", - ParameterValue{parameters.self_collision_proximity_threshold}, - ParameterDescriptorBuilder{} - .type(PARAMETER_DOUBLE) - .description("Start decelerating when a self-collision is this far [m]")); - node_parameters->declare_parameter( - ns + ".scene_collision_proximity_threshold", - ParameterValue{parameters.scene_collision_proximity_threshold}, - ParameterDescriptorBuilder{} - .type(PARAMETER_DOUBLE) - .description("Start decelerating when a scene collision is this far [m]")); -} - -ServoParameters ServoParameters::get( - const std::string & ns, - const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters) -{ - auto parameters = ServoParameters{}; - - // ROS Parameters - parameters.use_gazebo = node_parameters->get_parameter(ns + ".use_gazebo").as_bool(); - parameters.status_topic = node_parameters->get_parameter(ns + ".status_topic").as_string(); - - // Properties of incoming commands - parameters.cartesian_command_in_topic = - node_parameters->get_parameter(ns + ".cartesian_command_in_topic").as_string(); - parameters.joint_command_in_topic = - node_parameters->get_parameter(ns + ".joint_command_in_topic").as_string(); - parameters.robot_link_command_frame = node_parameters->get_parameter( - ns + ".robot_link_command_frame").as_string(); - parameters.command_in_type = node_parameters->get_parameter(ns + ".command_in_type").as_string(); - parameters.linear_scale = node_parameters->get_parameter(ns + ".scale.linear").as_double(); - parameters.rotational_scale = - node_parameters->get_parameter(ns + ".scale.rotational").as_double(); - parameters.joint_scale = node_parameters->get_parameter(ns + ".scale.joint").as_double(); - - // Properties of Servo calculations - parameters.override_velocity_scaling_factor = - node_parameters->get_parameter(ns + ".override_velocity_scaling_factor").as_double(); - - // Properties of outgoing commands - parameters.command_out_topic = - node_parameters->get_parameter(ns + ".command_out_topic").as_string(); - parameters.publish_period = node_parameters->get_parameter(ns + ".publish_period").as_double(); - parameters.command_out_type = - node_parameters->get_parameter(ns + ".command_out_type").as_string(); - parameters.publish_joint_positions = node_parameters->get_parameter( - ns + ".publish_joint_positions").as_bool(); - parameters.publish_joint_velocities = node_parameters->get_parameter( - ns + ".publish_joint_velocities").as_bool(); - parameters.publish_joint_accelerations = - node_parameters->get_parameter(ns + ".publish_joint_accelerations").as_bool(); - parameters.low_latency_mode = node_parameters->get_parameter(ns + ".low_latency_mode").as_bool(); - - // Incoming Joint State properties - parameters.joint_topic = node_parameters->get_parameter(ns + ".joint_topic").as_string(); - parameters.smoothing_filter_plugin_name = - node_parameters->get_parameter(ns + ".smoothing_filter_plugin_name").as_string(); - - // MoveIt properties - parameters.move_group_name = node_parameters->get_parameter(ns + ".move_group_name").as_string(); - parameters.planning_frame = node_parameters->get_parameter(ns + ".planning_frame").as_string(); - parameters.ee_frame_name = node_parameters->get_parameter(ns + ".ee_frame_name").as_string(); - parameters.is_primary_planning_scene_monitor = - node_parameters->get_parameter(ns + ".is_primary_planning_scene_monitor").as_bool(); - parameters.monitored_planning_scene_topic = - node_parameters->get_parameter(ns + ".monitored_planning_scene_topic").as_string(); - - // Stopping behaviour - parameters.incoming_command_timeout = node_parameters->get_parameter( - ns + ".incoming_command_timeout").as_double(); - parameters.num_outgoing_halt_msgs_to_publish = - node_parameters->get_parameter(ns + ".num_outgoing_halt_msgs_to_publish").as_int(); - parameters.halt_all_joints_in_joint_mode = - node_parameters->get_parameter(ns + ".halt_all_joints_in_joint_mode").as_bool(); - parameters.halt_all_joints_in_cartesian_mode = - node_parameters->get_parameter(ns + ".halt_all_joints_in_cartesian_mode").as_bool(); - - // Configure handling of singularities and joint limits - parameters.lower_singularity_threshold = - node_parameters->get_parameter(ns + ".lower_singularity_threshold").as_double(); - parameters.hard_stop_singularity_threshold = - node_parameters->get_parameter(ns + ".hard_stop_singularity_threshold").as_double(); - parameters.joint_limit_margin = - node_parameters->get_parameter(ns + ".joint_limit_margin").as_double(); - - if (node_parameters->has_parameter(ns + ".leaving_singularity_threshold_multiplier")) { - parameters.leaving_singularity_threshold_multiplier = - node_parameters->get_parameter(ns + ".leaving_singularity_threshold_multiplier").as_double(); - } else { - parameters.leaving_singularity_threshold_multiplier = 1.0; - RCLCPP_WARN( - LOGGER, "Using the deprecated type of servo singularity handling; add parameter " - "'leaving_singularity_threshold_multiplier' to define leaving singularity threshold. See " - "https://github.com/ros-planning/moveit2/pull/620 for more information."); - } - - // Collision checking - parameters.check_collisions = node_parameters->get_parameter(ns + ".check_collisions").as_bool(); - parameters.collision_check_rate = - node_parameters->get_parameter(ns + ".collision_check_rate").as_double(); - parameters.self_collision_proximity_threshold = - node_parameters->get_parameter(ns + ".self_collision_proximity_threshold").as_double(); - parameters.scene_collision_proximity_threshold = - node_parameters->get_parameter(ns + ".scene_collision_proximity_threshold").as_double(); - - return parameters; -} - -std::optional ServoParameters::validate(ServoParameters parameters) -{ - // Begin input checking - if (parameters.publish_period <= 0.) { - RCLCPP_WARN( - LOGGER, "Parameter 'publish_period' should be " - "greater than zero. Check yaml file."); - return std::nullopt; - } - if (parameters.num_outgoing_halt_msgs_to_publish < 0) { - RCLCPP_WARN( - LOGGER, - "Parameter 'num_outgoing_halt_msgs_to_publish' should be greater than zero. Check yaml file."); - return std::nullopt; - } - if (parameters.leaving_singularity_threshold_multiplier <= 0) { - RCLCPP_WARN( - LOGGER, - "Parameter 'leaving_singularity_threshold_multiplier' should be greater than zero. Check yaml file."); - return std::nullopt; - } - if (parameters.hard_stop_singularity_threshold <= parameters.lower_singularity_threshold) { - RCLCPP_WARN( - LOGGER, "Parameter 'hard_stop_singularity_threshold' " - "should be greater than 'lower_singularity_threshold.' " - "Check yaml file."); - return std::nullopt; - } - if ((parameters.hard_stop_singularity_threshold <= 0.) || - (parameters.lower_singularity_threshold <= 0.)) - { - RCLCPP_WARN( - LOGGER, "Parameters 'hard_stop_singularity_threshold' " - "and 'lower_singularity_threshold' should be " - "greater than zero. Check yaml file."); - return std::nullopt; - } - if (parameters.smoothing_filter_plugin_name.empty()) { - RCLCPP_WARN(LOGGER, "A smoothing plugin is required."); - return std::nullopt; - } - if (parameters.joint_limit_margin < 0.) { - RCLCPP_WARN( - LOGGER, "Parameter 'joint_limit_margin' should usually be greater than or equal to zero, " - "although negative values can be used if the specified joint limits are actually soft. " - "Check yaml file."); - } - if (parameters.command_in_type != "unitless" && parameters.command_in_type != "speed_units") { - RCLCPP_WARN( - LOGGER, "command_in_type should be 'unitless' or " - "'speed_units'. Check yaml file."); - return std::nullopt; - } - if (parameters.command_out_type != "trajectory_msgs/JointTrajectory" && - parameters.command_out_type != "std_msgs/Float64MultiArray") - { - RCLCPP_WARN( - LOGGER, "Parameter command_out_type should be " - "'trajectory_msgs/JointTrajectory' or " - "'std_msgs/Float64MultiArray'. Check yaml file."); - return std::nullopt; - } - if (!parameters.publish_joint_positions && !parameters.publish_joint_velocities && - !parameters.publish_joint_accelerations) - { - RCLCPP_WARN( - LOGGER, "At least one of publish_joint_positions / " - "publish_joint_velocities / " - "publish_joint_accelerations must be true. Check " - "yaml file."); - return std::nullopt; - } - if ((parameters.command_out_type == "std_msgs/Float64MultiArray") && - parameters.publish_joint_positions && - parameters.publish_joint_velocities) - { - RCLCPP_WARN( - LOGGER, "When publishing a std_msgs/Float64MultiArray, " - "you must select positions OR velocities."); - return std::nullopt; - } - // Collision checking - if (parameters.self_collision_proximity_threshold <= 0.) { - RCLCPP_WARN( - LOGGER, "Parameter 'self_collision_proximity_threshold' should be " - "greater than zero. Check yaml file."); - return std::nullopt; - } - if (parameters.scene_collision_proximity_threshold <= 0.) { - RCLCPP_WARN( - LOGGER, "Parameter 'scene_collision_proximity_threshold' should be " - "greater than zero. Check yaml file."); - return std::nullopt; - } - if (parameters.scene_collision_proximity_threshold < - parameters.self_collision_proximity_threshold) - { - RCLCPP_WARN( - LOGGER, "Parameter 'self_collision_proximity_threshold' should probably be less " - "than or equal to 'scene_collision_proximity_threshold'. Check yaml file."); - } - if (parameters.collision_check_rate <= 0) { - RCLCPP_WARN( - LOGGER, "Parameter 'collision_check_rate' should be " - "greater than zero. Check yaml file."); - return std::nullopt; - } - return parameters; -} - -ServoParameters::SharedConstPtr ServoParameters::makeServoParameters( - const rclcpp::Node::SharedPtr & node, - const std::string & ns, /* = "moveit_servo"*/ - bool dynamic_parameters /* = true */) -{ - auto node_parameters = node->get_node_parameters_interface(); - - // Get the parameters - declare(ns, node_parameters); - auto parameters = validate(get(ns, node_parameters)); - - if (parameters) { - auto parameters_ptr = std::make_shared(parameters.value()); - parameters_ptr->callback_handler_ = std::make_shared(); - - // register parameter change callback - if (dynamic_parameters) { - parameters_ptr->callback_handler_->on_set_parameters_callback_handler_ = - node->add_on_set_parameters_callback( - [ptr = - parameters_ptr->callback_handler_.get()]( - const std::vector & parameters) { - return ptr->setParametersCallback(parameters); - }); - } - - return parameters_ptr; - } - return nullptr; -} - -} // namespace moveit_servo diff --git a/urc_manipulation/src/utilities.cpp b/urc_manipulation/src/utilities.cpp deleted file mode 100644 index 8daa2bc1..00000000 --- a/urc_manipulation/src/utilities.cpp +++ /dev/null @@ -1,167 +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 - -namespace moveit_servo -{ -namespace -{ -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.utilities"); -constexpr auto ROS_LOG_THROTTLE_PERIOD = std::chrono::milliseconds(3000).count(); -} // namespace - -/** \brief Helper function for detecting zeroed message **/ -bool isNonZero(const geometry_msgs::msg::TwistStamped & msg) -{ - return msg.twist.linear.x != 0.0 || msg.twist.linear.y != 0.0 || msg.twist.linear.z != 0.0 || - msg.twist.angular.x != 0.0 || msg.twist.angular.y != 0.0 || msg.twist.angular.z != 0.0; -} - -/** \brief Helper function for detecting zeroed message **/ -bool isNonZero(const control_msgs::msg::JointJog & msg) -{ - bool all_zeros = true; - for (double delta : msg.velocities) { - all_zeros &= (delta == 0.0); - } - return !all_zeros; -} - -/** \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; -} - -/** \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] clock A ROS clock, for logging - * @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, rclcpp::Clock & clock, - 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; - RCLCPP_WARN_STREAM_THROTTLE( - LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - SERVO_STATUS_CODE_MAP.at(status)); - } - // Very close to singularity, so halt. - else if (ini_condition >= upper_threshold) { - velocity_scale = 0; - status = StatusCode::HALT_FOR_SINGULARITY; - RCLCPP_WARN_STREAM_THROTTLE( - LOGGER, clock, ROS_LOG_THROTTLE_PERIOD, - SERVO_STATUS_CODE_MAP.at(status)); - } - - return velocity_scale; -} - -} // namespace moveit_servo