diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt
index e94be4f0f..f310a32e8 100644
--- a/ur_controllers/CMakeLists.txt
+++ b/ur_controllers/CMakeLists.txt
@@ -8,6 +8,7 @@ endif()
find_package(ament_cmake REQUIRED)
find_package(angles REQUIRED)
find_package(controller_interface REQUIRED)
+find_package(geometry_msgs REQUIRED)
find_package(joint_trajectory_controller REQUIRED)
find_package(lifecycle_msgs REQUIRED)
find_package(pluginlib REQUIRED)
@@ -16,6 +17,8 @@ find_package(rcutils REQUIRED)
find_package(realtime_tools REQUIRED)
find_package(std_msgs REQUIRED)
find_package(std_srvs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
find_package(ur_dashboard_msgs REQUIRED)
find_package(ur_msgs REQUIRED)
find_package(generate_parameter_library REQUIRED)
@@ -27,6 +30,7 @@ find_package(action_msgs REQUIRED)
set(THIS_PACKAGE_INCLUDE_DEPENDS
angles
controller_interface
+ geometry_msgs
joint_trajectory_controller
lifecycle_msgs
pluginlib
@@ -35,6 +39,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
realtime_tools
std_msgs
std_srvs
+ tf2_geometry_msgs
+ tf2_ros
ur_dashboard_msgs
ur_msgs
generate_parameter_library
@@ -45,6 +51,10 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
include_directories(include)
+generate_parameter_library(
+ force_mode_controller_parameters
+ src/force_mode_controller_parameters.yaml
+)
generate_parameter_library(
gpio_controller_parameters
@@ -72,6 +82,7 @@ generate_parameter_library(
)
add_library(${PROJECT_NAME} SHARED
+ src/force_mode_controller.cpp
src/scaled_joint_trajectory_controller.cpp
src/speed_scaling_state_broadcaster.cpp
src/gpio_controller.cpp
@@ -82,6 +93,7 @@ target_include_directories(${PROJECT_NAME} PRIVATE
include
)
target_link_libraries(${PROJECT_NAME}
+ force_mode_controller_parameters
gpio_controller_parameters
speed_scaling_state_broadcaster_parameters
scaled_joint_trajectory_controller_parameters
@@ -124,4 +136,23 @@ ament_export_libraries(
${PROJECT_NAME}
)
+if(BUILD_TESTING)
+ find_package(ament_cmake_gmock REQUIRED)
+ find_package(controller_manager REQUIRED)
+ find_package(hardware_interface REQUIRED)
+ find_package(ros2_control_test_assets REQUIRED)
+
+ add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
+ ament_add_gmock(test_load_force_mode_controller
+ test/test_load_force_mode_controller.cpp
+ )
+ target_link_libraries(test_load_force_mode_controller
+ ${PROJECT_NAME}
+ )
+ ament_target_dependencies(test_load_force_mode_controller
+ controller_manager
+ ros2_control_test_assets
+ )
+endif()
+
ament_package()
diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml
index c784b35d0..f3365a26d 100644
--- a/ur_controllers/controller_plugins.xml
+++ b/ur_controllers/controller_plugins.xml
@@ -14,6 +14,11 @@
This controller publishes the Tool IO.
+
+
+ Controller to use UR's force_mode.
+
+
This controller forwards a joint-based trajectory to the robot controller for interpolation.
diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst
index a488d07e5..38211d083 100644
--- a/ur_controllers/doc/index.rst
+++ b/ur_controllers/doc/index.rst
@@ -264,3 +264,79 @@ Implementation details / dataflow
* When the hardware reports that execution has been aborted (The ``passthrough_trajectory_abort``
command interface), the action will be aborted.
* When the action is preempted, execution on the hardware is preempted.
+
+.. _force_mode_controller:
+
+ur_controllers/ForceModeController
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+This controller activates the robot's *Force Mode*. This allows direct force control running on the
+robot control box. This controller basically interfaces the URScript function ``force_mode(...)``.
+
+Force mode can be combined with (and only with) the :ref:`passthrough trajectory controller
+` in order to execute motions under a given force constraints.
+
+.. note::
+ This is not an admittance controller, as given force constraints in a certain Cartesian
+ dimension will overwrite the motion commands in that dimension. E.g. when specifying a certain
+ force in the base frame's ``z`` direction, any motion resulting from the move command in the
+ base frame's ``z`` axis will not be executed.
+
+Parameters
+""""""""""
+
++----------------------------------+--------+---------------+---------------------------------------------------------------------+
+| Parameter name | Type | Default value | Description |
+| | | | |
++----------------------------------+--------+---------------+---------------------------------------------------------------------+
+| ``tf_prefix`` | string | | Urdf prefix of the corresponding arm |
++----------------------------------+--------+---------------+---------------------------------------------------------------------+
+| ``check_io_successful_retries`` | int | 10 | Amount of retries for checking if setting force_mode was successful |
++----------------------------------+--------+---------------+---------------------------------------------------------------------+
+
+Service interface / usage
+"""""""""""""""""""""""""
+
+The controller provides two services: One for activating force_mode and one for leaving it. To use
+those services, the controller has to be in ``active`` state.
+
+* ``~/stop_force_mode [std_srvs/srv/Trigger]``: Stop force mode
+* ``~/start_force_mode [ur_msgs/srv/SetForceMode]``: Start force mode
+
+In ``ur_msgs/srv/SetForceMode`` the fields have the following meanings:
+
+task_frame
+ All information (selection vector, wrench, limits, etc) will be considered to be relative
+ to that pose. The pose's frame_id can be anything that is transformable to the robot's
+ ``base`` frame.
+selection_vector_
+ 1 means that the robot will be compliant in the corresponding axis of the task frame.
+wrench
+ The forces/torques the robot will apply to its environment. The robot adjusts its position
+ along/about compliant axis in order to achieve the specified force/torque. Values have no effect for non-
+ compliant axes.
+ Actual wrench applied may be lower than requested due to joint safety limits.
+type
+ An integer [1;3] specifying how the robot interprets the force frame
+
+ 1
+ The force frame is transformed in a way such that its y-axis is aligned with a vector pointing
+ from the robot tcp towards the origin of the force frame.
+ 2
+ The force frame is not transformed.
+ 3
+ The force frame is transformed in a way such that its x-axis is the projection of the robot tcp
+ velocity vector onto the x-y plane of the force frame.
+speed_limits
+ Maximum allowed tcp speed (relative to the task frame). This is **only relevant for axes marked as
+ compliant** in the selection_vector.
+deviation_limits
+ For **non-compliant axes**, these values are the maximum allowed deviation along/about an axis
+ between the actual tcp position and the one set by the program.
+damping_factor
+ Force mode damping factor. Sets the damping parameter in force mode. In range [0;1], default value is 0.025
+ A value of 1 is full damping, so the robot will decelerate quickly if no force is present. A value of 0
+ is no damping, here the robot will maintain the speed.
+gain_scaling
+ Force mode gain scaling factor. Scales the gain in force mode. scaling parameter is in range [0;2], default is 0.5.
+ A value larger than 1 can make force mode unstable, e.g. in case of collisions or pushing against hard surfaces.
diff --git a/ur_controllers/include/ur_controllers/force_mode_controller.hpp b/ur_controllers/include/ur_controllers/force_mode_controller.hpp
new file mode 100644
index 000000000..9ae45a04b
--- /dev/null
+++ b/ur_controllers/include/ur_controllers/force_mode_controller.hpp
@@ -0,0 +1,147 @@
+// Copyright 2023, FZI Forschungszentrum Informatik, Created on behalf of Universal Robots A/S
+//
+// 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.
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author Felix Exner exner@fzi.de
+ * \date 2023-06-29
+ */
+//----------------------------------------------------------------------
+
+#pragma once
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+#include "force_mode_controller_parameters.hpp"
+
+namespace ur_controllers
+{
+enum CommandInterfaces
+{
+ FORCE_MODE_TASK_FRAME_X = 0u,
+ FORCE_MODE_TASK_FRAME_Y = 1,
+ FORCE_MODE_TASK_FRAME_Z = 2,
+ FORCE_MODE_TASK_FRAME_RX = 3,
+ FORCE_MODE_TASK_FRAME_RY = 4,
+ FORCE_MODE_TASK_FRAME_RZ = 5,
+ FORCE_MODE_SELECTION_VECTOR_X = 6,
+ FORCE_MODE_SELECTION_VECTOR_Y = 7,
+ FORCE_MODE_SELECTION_VECTOR_Z = 8,
+ FORCE_MODE_SELECTION_VECTOR_RX = 9,
+ FORCE_MODE_SELECTION_VECTOR_RY = 10,
+ FORCE_MODE_SELECTION_VECTOR_RZ = 11,
+ FORCE_MODE_WRENCH_X = 12,
+ FORCE_MODE_WRENCH_Y = 13,
+ FORCE_MODE_WRENCH_Z = 14,
+ FORCE_MODE_WRENCH_RX = 15,
+ FORCE_MODE_WRENCH_RY = 16,
+ FORCE_MODE_WRENCH_RZ = 17,
+ FORCE_MODE_TYPE = 18,
+ FORCE_MODE_LIMITS_X = 19,
+ FORCE_MODE_LIMITS_Y = 20,
+ FORCE_MODE_LIMITS_Z = 21,
+ FORCE_MODE_LIMITS_RX = 22,
+ FORCE_MODE_LIMITS_RY = 23,
+ FORCE_MODE_LIMITS_RZ = 24,
+ FORCE_MODE_ASYNC_SUCCESS = 25,
+ FORCE_MODE_DISABLE_CMD = 26,
+ FORCE_MODE_DAMPING = 27,
+ FORCE_MODE_GAIN_SCALING = 28,
+};
+enum StateInterfaces
+{
+ INITIALIZED_FLAG = 0u,
+};
+
+struct ForceModeParameters
+{
+ std::array task_frame;
+ std::array selection_vec;
+ std::array limits;
+ geometry_msgs::msg::Wrench wrench;
+ double type;
+ double damping_factor;
+ double gain_scaling;
+};
+
+class ForceModeController : public controller_interface::ControllerInterface
+{
+public:
+ controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+ controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+ controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+
+ CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+
+ CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+
+ CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+ CallbackReturn on_init() override;
+
+ CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override;
+
+private:
+ bool setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req,
+ ur_msgs::srv::SetForceMode::Response::SharedPtr resp);
+ bool disableForceMode(const std_srvs::srv::Trigger::Request::SharedPtr req,
+ std_srvs::srv::Trigger::Response::SharedPtr resp);
+ rclcpp::Service::SharedPtr set_force_mode_srv_;
+ rclcpp::Service::SharedPtr disable_force_mode_srv_;
+
+ std::unique_ptr tf_buffer_;
+ std::unique_ptr tf_listener_;
+
+ std::shared_ptr param_listener_;
+ force_mode_controller::Params params_;
+
+ realtime_tools::RealtimeBuffer force_mode_params_buffer_;
+ std::atomic force_mode_active_;
+ std::atomic change_requested_;
+ std::atomic async_state_;
+
+ static constexpr double ASYNC_WAITING = 2.0;
+ /**
+ * @brief wait until a command interface isn't in state ASYNC_WAITING anymore or until the parameter maximum_retries
+ * have been reached
+ */
+ bool waitForAsyncCommand(std::function get_value);
+};
+} // namespace ur_controllers
diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml
index cae303417..c7f0e77b7 100644
--- a/ur_controllers/package.xml
+++ b/ur_controllers/package.xml
@@ -23,6 +23,7 @@
angles
controller_interface
+ geometry_msgs
joint_trajectory_controller
lifecycle_msgs
pluginlib
@@ -31,6 +32,8 @@
realtime_tools
std_msgs
std_srvs
+ tf2_geometry_msgs
+ tf2_ros
ur_dashboard_msgs
ur_msgs
control_msgs
diff --git a/ur_controllers/src/force_mode_controller.cpp b/ur_controllers/src/force_mode_controller.cpp
new file mode 100644
index 000000000..fcb2935b4
--- /dev/null
+++ b/ur_controllers/src/force_mode_controller.cpp
@@ -0,0 +1,380 @@
+// Copyright 2023, FZI Forschungszentrum Informatik, Created on behalf of Universal Robots A/S
+//
+// 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.
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author Felix Exner exner@fzi.de
+ * \date 2023-06-29
+ */
+//----------------------------------------------------------------------
+
+#include
+#include
+#include
+#include
+
+#include
+namespace ur_controllers
+{
+controller_interface::CallbackReturn ForceModeController::on_init()
+{
+ try {
+ // Create the parameter listener and get the parameters
+ param_listener_ = std::make_shared(get_node());
+ params_ = param_listener_->get_params();
+ } catch (const std::exception& e) {
+ fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+ return CallbackReturn::ERROR;
+ }
+
+ return controller_interface::CallbackReturn::SUCCESS;
+}
+controller_interface::InterfaceConfiguration ForceModeController::command_interface_configuration() const
+{
+ controller_interface::InterfaceConfiguration config;
+ config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+ const std::string tf_prefix = params_.tf_prefix;
+ RCLCPP_DEBUG(get_node()->get_logger(), "Configure UR force_mode controller with tf_prefix: %s", tf_prefix.c_str());
+
+ // Get all the command interfaces needed for force mode from the hardware interface
+ config.names.emplace_back(tf_prefix + "force_mode/task_frame_x");
+ config.names.emplace_back(tf_prefix + "force_mode/task_frame_y");
+ config.names.emplace_back(tf_prefix + "force_mode/task_frame_z");
+ config.names.emplace_back(tf_prefix + "force_mode/task_frame_rx");
+ config.names.emplace_back(tf_prefix + "force_mode/task_frame_ry");
+ config.names.emplace_back(tf_prefix + "force_mode/task_frame_rz");
+ config.names.emplace_back(tf_prefix + "force_mode/selection_vector_x");
+ config.names.emplace_back(tf_prefix + "force_mode/selection_vector_y");
+ config.names.emplace_back(tf_prefix + "force_mode/selection_vector_z");
+ config.names.emplace_back(tf_prefix + "force_mode/selection_vector_rx");
+ config.names.emplace_back(tf_prefix + "force_mode/selection_vector_ry");
+ config.names.emplace_back(tf_prefix + "force_mode/selection_vector_rz");
+ config.names.emplace_back(tf_prefix + "force_mode/wrench_x");
+ config.names.emplace_back(tf_prefix + "force_mode/wrench_y");
+ config.names.emplace_back(tf_prefix + "force_mode/wrench_z");
+ config.names.emplace_back(tf_prefix + "force_mode/wrench_rx");
+ config.names.emplace_back(tf_prefix + "force_mode/wrench_ry");
+ config.names.emplace_back(tf_prefix + "force_mode/wrench_rz");
+ config.names.emplace_back(tf_prefix + "force_mode/type");
+ config.names.emplace_back(tf_prefix + "force_mode/limits_x");
+ config.names.emplace_back(tf_prefix + "force_mode/limits_y");
+ config.names.emplace_back(tf_prefix + "force_mode/limits_z");
+ config.names.emplace_back(tf_prefix + "force_mode/limits_rx");
+ config.names.emplace_back(tf_prefix + "force_mode/limits_ry");
+ config.names.emplace_back(tf_prefix + "force_mode/limits_rz");
+ config.names.emplace_back(tf_prefix + "force_mode/force_mode_async_success");
+ config.names.emplace_back(tf_prefix + "force_mode/disable_cmd");
+ config.names.emplace_back(tf_prefix + "force_mode/damping");
+ config.names.emplace_back(tf_prefix + "force_mode/gain_scaling");
+
+ return config;
+}
+
+controller_interface::InterfaceConfiguration ur_controllers::ForceModeController::state_interface_configuration() const
+{
+ controller_interface::InterfaceConfiguration config;
+ config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+ const std::string tf_prefix = params_.tf_prefix;
+ // Get the state interface indicating whether the hardware interface has been initialized
+ config.names.emplace_back(tf_prefix + "system_interface/initialized");
+
+ return config;
+}
+
+controller_interface::CallbackReturn
+ur_controllers::ForceModeController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+ const auto logger = get_node()->get_logger();
+
+ if (!param_listener_) {
+ RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during configuration");
+ return controller_interface::CallbackReturn::ERROR;
+ }
+
+ // update the dynamic map parameters
+ param_listener_->refresh_dynamic_parameters();
+
+ // get parameters from the listener in case they were updated
+ params_ = param_listener_->get_params();
+
+ tf_buffer_ = std::make_unique(get_node()->get_clock());
+ tf_listener_ = std::make_unique(*tf_buffer_);
+
+ // Create the service server that will be used to start force mode
+ try {
+ set_force_mode_srv_ = get_node()->create_service(
+ "~/start_force_mode",
+ std::bind(&ForceModeController::setForceMode, this, std::placeholders::_1, std::placeholders::_2));
+ disable_force_mode_srv_ = get_node()->create_service(
+ "~/stop_force_mode",
+ std::bind(&ForceModeController::disableForceMode, this, std::placeholders::_1, std::placeholders::_2));
+ } catch (...) {
+ return LifecycleNodeInterface::CallbackReturn::ERROR;
+ }
+
+ return LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn
+ur_controllers::ForceModeController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+ change_requested_ = false;
+ force_mode_active_ = false;
+ async_state_ = std::numeric_limits::quiet_NaN();
+ return LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn
+ur_controllers::ForceModeController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+ // Stop force mode if this controller is deactivated.
+ command_interfaces_[CommandInterfaces::FORCE_MODE_DISABLE_CMD].set_value(1.0);
+ return LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn
+ur_controllers::ForceModeController::on_cleanup(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+ set_force_mode_srv_.reset();
+ disable_force_mode_srv_.reset();
+ return CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type ur_controllers::ForceModeController::update(const rclcpp::Time& /*time*/,
+ const rclcpp::Duration& /*period*/)
+{
+ async_state_ = command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value();
+
+ // Publish state of force_mode?
+ if (change_requested_) {
+ if (force_mode_active_) {
+ const auto force_mode_parameters = force_mode_params_buffer_.readFromRT();
+ command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_X].set_value(force_mode_parameters->task_frame[0]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Y].set_value(force_mode_parameters->task_frame[1]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Z].set_value(force_mode_parameters->task_frame[2]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RX].set_value(force_mode_parameters->task_frame[3]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RY].set_value(force_mode_parameters->task_frame[4]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RZ].set_value(force_mode_parameters->task_frame[5]);
+
+ command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_X].set_value(
+ force_mode_parameters->selection_vec[0]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Y].set_value(
+ force_mode_parameters->selection_vec[1]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Z].set_value(
+ force_mode_parameters->selection_vec[2]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RX].set_value(
+ force_mode_parameters->selection_vec[3]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RY].set_value(
+ force_mode_parameters->selection_vec[4]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RZ].set_value(
+ force_mode_parameters->selection_vec[5]);
+
+ command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_X].set_value(force_mode_parameters->wrench.force.x);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Y].set_value(force_mode_parameters->wrench.force.y);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Z].set_value(force_mode_parameters->wrench.force.z);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RX].set_value(force_mode_parameters->wrench.torque.x);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RY].set_value(force_mode_parameters->wrench.torque.y);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RZ].set_value(force_mode_parameters->wrench.torque.z);
+
+ command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X].set_value(force_mode_parameters->limits[0]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Y].set_value(force_mode_parameters->limits[1]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Z].set_value(force_mode_parameters->limits[2]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RX].set_value(force_mode_parameters->limits[3]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RY].set_value(force_mode_parameters->limits[4]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RZ].set_value(force_mode_parameters->limits[5]);
+
+ command_interfaces_[CommandInterfaces::FORCE_MODE_TYPE].set_value(force_mode_parameters->type);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_DAMPING].set_value(force_mode_parameters->damping_factor);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_GAIN_SCALING].set_value(force_mode_parameters->gain_scaling);
+
+ // Signal that we are waiting for confirmation that force mode is activated
+ command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+ async_state_ = ASYNC_WAITING;
+ } else {
+ command_interfaces_[CommandInterfaces::FORCE_MODE_DISABLE_CMD].set_value(1.0);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+ async_state_ = ASYNC_WAITING;
+ }
+ change_requested_ = false;
+ }
+
+ return controller_interface::return_type::OK;
+}
+
+bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req,
+ ur_msgs::srv::SetForceMode::Response::SharedPtr resp)
+{
+ // Reject if controller is not active
+ if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
+ RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new action goals. Controller is not running.");
+ resp->success = false;
+ return false;
+ }
+
+ ForceModeParameters force_mode_parameters;
+
+ // transform task frame into base
+ const std::string tf_prefix = params_.tf_prefix;
+ if (std::abs(req->task_frame.pose.orientation.x) < 1e-6 && std::abs(req->task_frame.pose.orientation.y) < 1e-6 &&
+ std::abs(req->task_frame.pose.orientation.z) < 1e-6 && std::abs(req->task_frame.pose.orientation.w) < 1e-6) {
+ RCLCPP_ERROR(get_node()->get_logger(), "Received task frame with all-zeros quaternion. It should have at least one "
+ "non-zero entry.");
+ resp->success = false;
+ return false;
+ }
+
+ try {
+ auto task_frame_transformed = tf_buffer_->transform(req->task_frame, tf_prefix + "base");
+
+ force_mode_parameters.task_frame[0] = task_frame_transformed.pose.position.x;
+ force_mode_parameters.task_frame[1] = task_frame_transformed.pose.position.y;
+ force_mode_parameters.task_frame[2] = task_frame_transformed.pose.position.z;
+
+ tf2::Quaternion quat_tf;
+ tf2::convert(task_frame_transformed.pose.orientation, quat_tf);
+ tf2::Matrix3x3 rot_mat(quat_tf);
+ rot_mat.getRPY(force_mode_parameters.task_frame[3], force_mode_parameters.task_frame[4],
+ force_mode_parameters.task_frame[5]);
+ } catch (const tf2::TransformException& ex) {
+ RCLCPP_ERROR(get_node()->get_logger(), "Could not transform %s to robot base: %s",
+ req->task_frame.header.frame_id.c_str(), ex.what());
+ resp->success = false;
+ return false;
+ }
+
+ // The selection vector dictates which axes the robot should be compliant along and around
+ force_mode_parameters.selection_vec[0] = req->selection_vector_x;
+ force_mode_parameters.selection_vec[1] = req->selection_vector_y;
+ force_mode_parameters.selection_vec[2] = req->selection_vector_z;
+ force_mode_parameters.selection_vec[3] = req->selection_vector_rx;
+ force_mode_parameters.selection_vec[4] = req->selection_vector_ry;
+ force_mode_parameters.selection_vec[5] = req->selection_vector_rz;
+
+ // The wrench parameters dictate the amount of force/torque the robot will apply to its environment. The robot will
+ // move along/around compliant axes to match the specified force/torque. Has no effect for non-compliant axes.
+ force_mode_parameters.wrench = req->wrench;
+
+ /* The limits specifies the maximum allowed speed along/around compliant axes. For non-compliant axes this value is
+ * the maximum allowed deviation between actual tcp position and the one that has been programmed. */
+ force_mode_parameters.limits[0] = req->selection_vector_x ? req->speed_limits.linear.x : req->deviation_limits[0];
+ force_mode_parameters.limits[1] = req->selection_vector_y ? req->speed_limits.linear.y : req->deviation_limits[1];
+ force_mode_parameters.limits[2] = req->selection_vector_z ? req->speed_limits.linear.z : req->deviation_limits[2];
+ force_mode_parameters.limits[3] = req->selection_vector_rx ? req->speed_limits.angular.x : req->deviation_limits[3];
+ force_mode_parameters.limits[4] = req->selection_vector_ry ? req->speed_limits.angular.y : req->deviation_limits[4];
+ force_mode_parameters.limits[5] = req->selection_vector_rz ? req->speed_limits.angular.z : req->deviation_limits[5];
+
+ if (req->type < 1 || req->type > 3) {
+ RCLCPP_ERROR(get_node()->get_logger(), "The force mode type has to be 1, 2, or 3. Received %u", req->type);
+ resp->success = false;
+ return false;
+ }
+
+ /* The type decides how the robot interprets the force frame (the one defined in task_frame). See ur_script manual
+ * for explanation, under force_mode. */
+ force_mode_parameters.type = static_cast(req->type);
+
+ /* The damping factor decides how fast the robot decelarates if no force is present. 0 means no deceleration, 1
+ * means quick deceleration*/
+ if (req->damping_factor < 0.0 || req->damping_factor > 1.0) {
+ RCLCPP_ERROR(get_node()->get_logger(), "The damping factor has to be between 0 and 1. Received %f",
+ req->damping_factor);
+ resp->success = false;
+ return false;
+ }
+ force_mode_parameters.damping_factor = req->damping_factor;
+
+ /*The gain scaling factor scales the force mode gain. A value larger than 1 may make force mode unstable. */
+ if (req->gain_scaling < 0.0 || req->gain_scaling > 2.0) {
+ RCLCPP_ERROR(get_node()->get_logger(), "The gain scaling has to be between 0 and 2. Received %f",
+ req->gain_scaling);
+ resp->success = false;
+ return false;
+ }
+ if (req->gain_scaling > 1.0) {
+ RCLCPP_WARN(get_node()->get_logger(),
+ "A gain_scaling >1.0 can make force mode unstable, e.g. in case of collisions or pushing against "
+ "hard surfaces. Received %f",
+ req->gain_scaling);
+ }
+ force_mode_parameters.gain_scaling = req->gain_scaling;
+
+ force_mode_params_buffer_.writeFromNonRT(force_mode_parameters);
+ force_mode_active_ = true;
+ change_requested_ = true;
+
+ RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for force mode to be set.");
+ const auto maximum_retries = params_.check_io_successful_retries;
+ int retries = 0;
+ while (async_state_ == ASYNC_WAITING || change_requested_) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(10));
+ retries++;
+
+ if (retries > maximum_retries) {
+ resp->success = false;
+ }
+ }
+
+ resp->success = async_state_ == 1.0;
+
+ if (resp->success) {
+ RCLCPP_INFO(get_node()->get_logger(), "Force mode has been set successfully.");
+ } else {
+ RCLCPP_ERROR(get_node()->get_logger(), "Could not set the force mode.");
+ return false;
+ }
+
+ return true;
+}
+
+bool ForceModeController::disableForceMode(const std_srvs::srv::Trigger::Request::SharedPtr /*req*/,
+ std_srvs::srv::Trigger::Response::SharedPtr resp)
+{
+ force_mode_active_ = false;
+ change_requested_ = true;
+ RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for force mode to be disabled.");
+ while (async_state_ == ASYNC_WAITING || change_requested_) {
+ // Asynchronous wait until the hardware interface has set the force mode
+ std::this_thread::sleep_for(std::chrono::milliseconds(10));
+ }
+ resp->success = async_state_ == 1.0;
+ if (resp->success) {
+ RCLCPP_INFO(get_node()->get_logger(), "Force mode has been disabled successfully.");
+ } else {
+ RCLCPP_ERROR(get_node()->get_logger(), "Could not disable force mode.");
+ return false;
+ }
+ return true;
+}
+} // namespace ur_controllers
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(ur_controllers::ForceModeController, controller_interface::ControllerInterface)
diff --git a/ur_controllers/src/force_mode_controller_parameters.yaml b/ur_controllers/src/force_mode_controller_parameters.yaml
new file mode 100644
index 000000000..4eb69efbe
--- /dev/null
+++ b/ur_controllers/src/force_mode_controller_parameters.yaml
@@ -0,0 +1,12 @@
+---
+force_mode_controller:
+ tf_prefix: {
+ type: string,
+ default_value: "",
+ description: "Urdf prefix of the corresponding arm"
+ }
+ check_io_successful_retries: {
+ type: int,
+ default_value: 10,
+ description: "Amount of retries for checking if setting force_mode was successful"
+ }
diff --git a/ur_controllers/test/force_mode_controller_params.yaml b/ur_controllers/test/force_mode_controller_params.yaml
new file mode 100644
index 000000000..648b9195c
--- /dev/null
+++ b/ur_controllers/test/force_mode_controller_params.yaml
@@ -0,0 +1,7 @@
+---
+force_mode_controller:
+ ros__parameters:
+ tf_prefix: ""
+ damping: 0.025
+ gain_scaling: 0.5
+ check_io_successful_retries: 10
diff --git a/ur_controllers/test/test_load_force_mode_controller.cpp b/ur_controllers/test/test_load_force_mode_controller.cpp
new file mode 100644
index 000000000..8f25b96e5
--- /dev/null
+++ b/ur_controllers/test/test_load_force_mode_controller.cpp
@@ -0,0 +1,60 @@
+// Copyright 2024, Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+//
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// * Neither the name of the {copyright_holder} nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include
+#include "controller_manager/controller_manager.hpp"
+#include "rclcpp/executor.hpp"
+#include "rclcpp/executors/single_threaded_executor.hpp"
+#include "rclcpp/utilities.hpp"
+#include "ros2_control_test_assets/descriptions.hpp"
+
+TEST(TestLoadForceModeController, load_controller)
+{
+ std::shared_ptr executor = std::make_shared();
+
+ controller_manager::ControllerManager cm{ executor, ros2_control_test_assets::minimal_robot_urdf, true,
+ "test_controller_manager" };
+
+ const std::string test_file_path = std::string{ TEST_FILES_DIRECTORY } + "/force_mode_controller_params.yaml";
+ cm.set_parameter({ "test_force_mode_controller.params_file", test_file_path });
+
+ cm.set_parameter({ "test_force_mode_controller.type", "ur_controllers/ForceModeController" });
+
+ ASSERT_NE(cm.load_controller("test_force_mode_controller"), nullptr);
+}
+
+int main(int argc, char* argv[])
+{
+ ::testing::InitGoogleMock(&argc, argv);
+ rclcpp::init(argc, argv);
+
+ int result = RUN_ALL_TESTS();
+ rclcpp::shutdown();
+
+ return result;
+}
diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt
index 0d0487a03..05b59e628 100644
--- a/ur_robot_driver/CMakeLists.txt
+++ b/ur_robot_driver/CMakeLists.txt
@@ -33,6 +33,7 @@ find_package(std_srvs REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(ur_client_library REQUIRED)
find_package(ur_dashboard_msgs REQUIRED)
+find_package(ur_msgs REQUIRED)
include_directories(include)
@@ -49,6 +50,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
tf2_geometry_msgs
ur_client_library
ur_dashboard_msgs
+ ur_msgs
)
add_library(ur_robot_driver_plugin
@@ -148,6 +150,8 @@ install(PROGRAMS
scripts/wait_for_robot_description
scripts/example_move.py
scripts/start_ursim.sh
+ examples/examples.py
+ examples/force_mode.py
DESTINATION lib/${PROJECT_NAME}
)
@@ -188,6 +192,10 @@ if(BUILD_TESTING)
TIMEOUT
800
)
+ add_launch_test(test/integration_test_force_mode.py
+ TIMEOUT
+ 800
+ )
add_launch_test(test/urscript_interface.py
TIMEOUT
500
diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml
index 182d6eac5..232e081e1 100644
--- a/ur_robot_driver/config/ur_controllers.yaml
+++ b/ur_robot_driver/config/ur_controllers.yaml
@@ -24,8 +24,12 @@ controller_manager:
forward_position_controller:
type: position_controllers/JointGroupPositionController
+ force_mode_controller:
+ type: ur_controllers/ForceModeController
+
passthrough_trajectory_controller:
type: ur_controllers/PassthroughTrajectoryController
+
tcp_pose_broadcaster:
type: pose_broadcaster/PoseBroadcaster
@@ -151,6 +155,10 @@ forward_position_controller:
- $(var tf_prefix)wrist_2_joint
- $(var tf_prefix)wrist_3_joint
+force_mode_controller:
+ ros__parameters:
+ tf_prefix: "$(var tf_prefix)"
+
tcp_pose_broadcaster:
ros__parameters:
frame_id: $(var tf_prefix)base
diff --git a/ur_robot_driver/doc/usage/controllers.rst b/ur_robot_driver/doc/usage/controllers.rst
index d1566758b..a35a8b084 100644
--- a/ur_robot_driver/doc/usage/controllers.rst
+++ b/ur_robot_driver/doc/usage/controllers.rst
@@ -110,3 +110,10 @@ Type: `position_controllers/JointGroupPositionController `
+
+Allows utilizing the robot's builtin *Force Mode*.
diff --git a/ur_robot_driver/examples/examples.py b/ur_robot_driver/examples/examples.py
new file mode 100644
index 000000000..59336cc6a
--- /dev/null
+++ b/ur_robot_driver/examples/examples.py
@@ -0,0 +1,175 @@
+#!/usr/bin/env python3
+# Copyright 2024, Universal Robots A/S
+#
+# 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.
+
+import rclpy
+from builtin_interfaces.msg import Duration
+from control_msgs.action import FollowJointTrajectory
+
+from rclpy.action import ActionClient
+from rclpy.node import Node
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from ur_msgs.srv import SetIO
+from controller_manager_msgs.srv import SwitchController
+from std_srvs.srv import Trigger
+
+TIMEOUT_WAIT_SERVICE = 10
+TIMEOUT_WAIT_SERVICE_INITIAL = 60
+TIMEOUT_WAIT_ACTION = 10
+
+ROBOT_JOINTS = [
+ "shoulder_pan_joint",
+ "shoulder_lift_joint",
+ "elbow_joint",
+ "wrist_1_joint",
+ "wrist_2_joint",
+ "wrist_3_joint",
+]
+
+
+# Helper functions
+def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE):
+ client = node.create_client(srv_type, srv_name)
+ if client.wait_for_service(timeout) is False:
+ raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}")
+
+ node.get_logger().info(f"Successfully connected to service '{srv_name}'")
+ return client
+
+
+def waitForAction(node, action_name, action_type, timeout=TIMEOUT_WAIT_ACTION):
+ client = ActionClient(node, action_type, action_name)
+ if client.wait_for_server(timeout) is False:
+ raise Exception(
+ f"Could not reach action server '{action_name}' within timeout of {timeout}"
+ )
+
+ node.get_logger().info(f"Successfully connected to action '{action_name}'")
+ return client
+
+
+class Robot:
+ def __init__(self, node):
+ self.node = node
+ self.service_interfaces = {
+ "/io_and_status_controller/set_io": SetIO,
+ "/dashboard_client/play": Trigger,
+ "/controller_manager/switch_controller": SwitchController,
+ }
+ self.init_robot()
+
+ def init_robot(self):
+ self.service_clients = {
+ srv_name: waitForService(self.node, srv_name, srv_type)
+ for (srv_name, srv_type) in self.service_interfaces.items()
+ }
+
+ self.jtc_action_client = waitForAction(
+ self.node,
+ "/scaled_joint_trajectory_controller/follow_joint_trajectory",
+ FollowJointTrajectory,
+ )
+
+ def set_io(self, pin, value):
+ """Test to set an IO."""
+ set_io_req = SetIO.Request()
+ set_io_req.fun = 1
+ set_io_req.pin = pin
+ set_io_req.state = value
+
+ self.call_service("/io_and_status_controller/set_io", set_io_req)
+
+ def send_trajectory(self, waypts, time_vec):
+ """Send robot trajectory."""
+ if len(waypts) != len(time_vec):
+ raise Exception("waypoints vector and time vec should be same length")
+
+ # Construct test trajectory
+ joint_trajectory = JointTrajectory()
+ joint_trajectory.joint_names = ROBOT_JOINTS
+ for i in range(len(waypts)):
+ point = JointTrajectoryPoint()
+ point.positions = waypts[i]
+ point.time_from_start = time_vec[i]
+ joint_trajectory.points.append(point)
+
+ # Sending trajectory goal
+ goal_response = self.call_action(
+ self.jtc_action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory)
+ )
+ if not goal_response.accepted:
+ raise Exception("trajectory was not accepted")
+
+ # Verify execution
+ result = self.get_result(self.jtc_action_client, goal_response)
+ return result.error_code == FollowJointTrajectory.Result.SUCCESSFUL
+
+ def call_service(self, srv_name, request):
+ future = self.service_clients[srv_name].call_async(request)
+ rclpy.spin_until_future_complete(self.node, future)
+ if future.result() is not None:
+ return future.result()
+ else:
+ raise Exception(f"Exception while calling service: {future.exception()}")
+
+ def call_action(self, ac_client, g):
+ future = ac_client.send_goal_async(g)
+ rclpy.spin_until_future_complete(self.node, future)
+
+ if future.result() is not None:
+ return future.result()
+ else:
+ raise Exception(f"Exception while calling action: {future.exception()}")
+
+ def get_result(self, ac_client, goal_response):
+ future_res = ac_client._get_result_async(goal_response)
+ rclpy.spin_until_future_complete(self.node, future_res)
+ if future_res.result() is not None:
+ return future_res.result().result
+ else:
+ raise Exception(f"Exception while calling action: {future_res.exception()}")
+
+
+if __name__ == "__main__":
+ rclpy.init()
+ node = Node("robot_driver_test")
+ robot = Robot(node)
+
+ # The following list are arbitrary joint positions, change according to your own needs
+ waypts = [
+ [-1.6006, -1.7272, -2.2030, -0.8079, 1.5951, -0.0311],
+ [-1.2, -1.4, -1.9, -1.2, 1.5951, -0.0311],
+ [-1.6006, -1.7272, -2.2030, -0.8079, 1.5951, -0.0311],
+ ]
+ time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)]
+
+ # Execute trajectory on robot, make sure that the robot is booted and the control script is running
+ robot.send_trajectory(waypts, time_vec)
+
+ # Set digital output 1 to true
+ robot.set_io(1, 1.0)
diff --git a/ur_robot_driver/examples/force_mode.py b/ur_robot_driver/examples/force_mode.py
new file mode 100644
index 000000000..9e65c020d
--- /dev/null
+++ b/ur_robot_driver/examples/force_mode.py
@@ -0,0 +1,136 @@
+#!/usr/bin/env python3
+# Copyright 2024, Universal Robots A/S
+#
+# 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.
+
+import time
+
+import rclpy
+from rclpy.node import Node
+from controller_manager_msgs.srv import SwitchController
+from builtin_interfaces.msg import Duration
+from std_msgs.msg import Header
+from std_srvs.srv import Trigger
+
+from geometry_msgs.msg import (
+ Point,
+ Quaternion,
+ Pose,
+ PoseStamped,
+ Wrench,
+ WrenchStamped,
+ Vector3,
+)
+
+from ur_msgs.srv import SetForceMode
+
+from examples import Robot
+
+if __name__ == "__main__":
+ rclpy.init()
+ node = Node("robot_driver_test")
+ robot = Robot(node)
+
+ # Activate force mode controller
+ robot.call_service(
+ "/controller_manager/switch_controller",
+ SwitchController.Request(
+ activate_controllers=["force_mode_controller", "scaled_joint_trajectory_controller"],
+ strictness=SwitchController.Request.BEST_EFFORT,
+ ),
+ )
+
+ # Add force mode service to service interfaces and re-init robot
+ robot.service_interfaces.update({"/force_mode_controller/start_force_mode": SetForceMode})
+ robot.init_robot()
+ time.sleep(2)
+ # Press play on the robot
+ robot.call_service("/dashboard_client/play", Trigger.Request())
+
+ # Move robot in to position
+ robot.send_trajectory(
+ waypts=[[-1.5707, -1.5707, -1.5707, -1.5707, 1.5707, 0]],
+ time_vec=[Duration(sec=6, nanosec=0)],
+ )
+
+ # Finished moving
+
+ # Create task frame for force mode
+ point = Point(x=0.0, y=0.0, z=0.0)
+ orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=0.0)
+ task_frame_pose = Pose()
+ task_frame_pose.position = point
+ task_frame_pose.orientation = orientation
+ header = Header(seq=1, frame_id="world")
+ header.stamp.sec = int(time.time()) + 1
+ header.stamp.nanosec = 0
+ frame_stamp = PoseStamped()
+ frame_stamp.header = header
+ frame_stamp.pose = task_frame_pose
+
+ # Create compliance vector (which axes should be force controlled)
+ compliance = [False, False, True, False, False, False]
+
+ # Create Wrench message for force mode
+ wrench_vec = Wrench(force=Vector3(x=0.0, y=0.0, z=-10.0), torque=Vector3(x=0.0, y=0.0, z=0.0))
+ wrench_stamp = WrenchStamped(header=header, wrench=wrench_vec)
+ # Specify interpretation of task frame (no transform)
+ type_spec = SetForceMode.Request.NO_TRANSFORM
+
+ # Specify max speeds and deviations of force mode
+ limits = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+
+ # specify damping and gain scaling
+ damping_factor = 0.025
+ gain_scale = 0.5
+
+ req = SetForceMode.Request()
+ req.task_frame = frame_stamp
+ req.selection_vector_x = compliance[0]
+ req.selection_vector_y = compliance[1]
+ req.selection_vector_z = compliance[2]
+ req.selection_vector_rx = compliance[3]
+ req.selection_vector_ry = compliance[4]
+ req.selection_vector_rz = compliance[5]
+ req.wrench = wrench_stamp
+ req.type = type_spec
+ req.limits = limits
+ req.damping_factor = damping_factor
+ req.gain_scaling = gain_scale
+
+ # Send request to controller
+ robot.call_service("/force_mode_controller/start_force_mode", req)
+
+ time.sleep(15)
+ # Deactivate force mode controller
+ robot.call_service(
+ "/controller_manager/switch_controller",
+ SwitchController.Request(
+ deactivate_controllers=["force_mode_controller"],
+ strictness=SwitchController.Request.BEST_EFFORT,
+ ),
+ )
diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp
index 428456113..1f665a1b2 100644
--- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp
+++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp
@@ -78,7 +78,8 @@ enum StoppingInterface
NONE,
STOP_POSITION,
STOP_VELOCITY,
- STOP_PASSTHROUGH
+ STOP_PASSTHROUGH,
+ STOP_FORCE_MODE
};
// We define our own quaternion to use it as a buffer, since we need to pass pointers to the state
@@ -158,6 +159,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
void updateNonDoubleValues();
void extractToolPose();
void transformForceTorque();
+ void start_force_mode();
+ void stop_force_mode();
void check_passthrough_trajectory_controller();
void trajectory_done_callback(urcl::control::TrajectoryResult result);
bool has_accelerations(std::vector> accelerations);
@@ -238,6 +241,18 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
double payload_mass_;
double payload_async_success_;
+ // force mode parameters
+ urcl::vector6d_t force_mode_task_frame_;
+ urcl::vector6d_t force_mode_selection_vector_;
+ urcl::vector6uint32_t force_mode_selection_vector_copy_;
+ urcl::vector6d_t force_mode_wrench_;
+ urcl::vector6d_t force_mode_limits_;
+ double force_mode_type_;
+ double force_mode_async_success_;
+ double force_mode_disable_cmd_;
+ double force_mode_damping_;
+ double force_mode_gain_scaling_;
+
// copy of non double values
std::array actual_dig_out_bits_copy_;
std::array actual_dig_in_bits_copy_;
@@ -265,10 +280,11 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
double pausing_ramp_up_increment_;
// resources switching aux vars
- std::vector stop_modes_;
- std::vector start_modes_;
+ std::vector> stop_modes_;
+ std::vector> start_modes_;
bool position_controller_running_;
bool velocity_controller_running_;
+ bool force_mode_controller_running_ = false;
std::unique_ptr ur_driver_;
std::shared_ptr async_thread_;
@@ -277,6 +293,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
urcl::RobotReceiveTimeout receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20);
+ const std::string FORCE_MODE_GPIO = "force_mode";
const std::string PASSTHROUGH_GPIO = "trajectory_passthrough";
};
} // namespace ur_robot_driver
diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py
index 9e1ef380b..8804cc4eb 100644
--- a/ur_robot_driver/launch/ur_control.launch.py
+++ b/ur_robot_driver/launch/ur_control.launch.py
@@ -175,6 +175,7 @@ def controller_spawner(controllers, active=True):
"joint_trajectory_controller",
"forward_velocity_controller",
"forward_position_controller",
+ "force_mode_controller",
"passthrough_trajectory_controller",
]
if activate_joint_controller.perform(context) == "true":
diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp
index 3fe6dbec3..88438b07a 100644
--- a/ur_robot_driver/src/hardware_interface.cpp
+++ b/ur_robot_driver/src/hardware_interface.cpp
@@ -81,9 +81,6 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys
urcl_position_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
urcl_position_commands_old_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
- stop_modes_ = { StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE,
- StoppingInterface::NONE, StoppingInterface::NONE, StoppingInterface::NONE };
- start_modes_ = {};
position_controller_running_ = false;
velocity_controller_running_ = false;
passthrough_trajectory_controller_running_ = false;
@@ -317,6 +314,36 @@ std::vector URPositionHardwareInterface::e
command_interfaces.emplace_back(
hardware_interface::CommandInterface(tf_prefix + "payload", "payload_async_success", &payload_async_success_));
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "task_frame_x", &force_mode_task_frame_[0]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "task_frame_y", &force_mode_task_frame_[1]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "task_frame_z", &force_mode_task_frame_[2]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "task_frame_rx", &force_mode_task_frame_[3]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "task_frame_ry", &force_mode_task_frame_[4]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "task_frame_rz", &force_mode_task_frame_[5]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "selection_vector_x", &force_mode_selection_vector_[0]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "selection_vector_y", &force_mode_selection_vector_[1]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "selection_vector_z", &force_mode_selection_vector_[2]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "selection_vector_rx", &force_mode_selection_vector_[3]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "selection_vector_ry", &force_mode_selection_vector_[4]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "selection_vector_rz", &force_mode_selection_vector_[5]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "wrench_x", &force_mode_wrench_[0]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "wrench_y", &force_mode_wrench_[1]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "wrench_z", &force_mode_wrench_[2]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "wrench_rx", &force_mode_wrench_[3]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "wrench_ry", &force_mode_wrench_[4]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "wrench_rz", &force_mode_wrench_[5]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "type", &force_mode_type_);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "limits_x", &force_mode_limits_[0]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "limits_y", &force_mode_limits_[1]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "limits_z", &force_mode_limits_[2]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "limits_rx", &force_mode_limits_[3]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "limits_ry", &force_mode_limits_[4]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "limits_rz", &force_mode_limits_[5]);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "force_mode_async_success", &force_mode_async_success_);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "disable_cmd", &force_mode_disable_cmd_);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "damping", &force_mode_damping_);
+ command_interfaces.emplace_back(tf_prefix + FORCE_MODE_GPIO, "gain_scaling", &force_mode_gain_scaling_);
+
for (size_t i = 0; i < 18; ++i) {
command_interfaces.emplace_back(hardware_interface::CommandInterface(
tf_prefix + "gpio", "standard_digital_output_cmd_" + std::to_string(i), &standard_dig_out_bits_cmd_[i]));
@@ -541,6 +568,14 @@ hardware_interface::CallbackReturn
URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous_state)
{
RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Activating HW interface");
+
+ for (size_t i = 0; i < 6; i++) {
+ force_mode_task_frame_[i] = NO_NEW_CMD_;
+ force_mode_selection_vector_[i] = static_cast(NO_NEW_CMD_);
+ force_mode_wrench_[i] = NO_NEW_CMD_;
+ force_mode_limits_[i] = NO_NEW_CMD_;
+ }
+ force_mode_type_ = static_cast(NO_NEW_CMD_);
return hardware_interface::CallbackReturn::SUCCESS;
}
@@ -680,6 +715,7 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::
resend_robot_program_cmd_ = NO_NEW_CMD_;
zero_ftsensor_cmd_ = NO_NEW_CMD_;
hand_back_control_cmd_ = NO_NEW_CMD_;
+ force_mode_disable_cmd_ = NO_NEW_CMD_;
initialized_ = true;
}
@@ -713,6 +749,14 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp:
check_passthrough_trajectory_controller();
} else {
ur_driver_->writeKeepalive();
+
+ if (!std::isnan(force_mode_task_frame_[0]) && !std::isnan(force_mode_selection_vector_[0]) &&
+ !std::isnan(force_mode_wrench_[0]) && !std::isnan(force_mode_type_) && !std::isnan(force_mode_limits_[0]) &&
+ !std::isnan(force_mode_damping_) && !std::isnan(force_mode_gain_scaling_) && ur_driver_ != nullptr) {
+ start_force_mode();
+ } else if (!std::isnan(force_mode_disable_cmd_) && ur_driver_ != nullptr && force_mode_async_success_ == 2.0) {
+ stop_force_mode();
+ }
}
packet_read_ = false;
@@ -880,26 +924,29 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
{
hardware_interface::return_type ret_val = hardware_interface::return_type::OK;
- start_modes_ = std::vector(info_.joints.size(), "UNDEFINED");
- stop_modes_.clear();
- std::vector control_modes(info_.joints.size());
+ start_modes_ = std::vector>(info_.joints.size());
+ stop_modes_ = std::vector>(info_.joints.size());
+ std::vector> control_modes(info_.joints.size());
const std::string tf_prefix = info_.hardware_parameters.at("tf_prefix");
// Assess current state
for (auto i = 0u; i < info_.joints.size(); i++) {
if (position_controller_running_) {
- control_modes[i] = hardware_interface::HW_IF_POSITION;
+ control_modes[i] = { hardware_interface::HW_IF_POSITION };
}
if (velocity_controller_running_) {
- control_modes[i] = hardware_interface::HW_IF_VELOCITY;
+ control_modes[i] = { hardware_interface::HW_IF_VELOCITY };
+ }
+ if (force_mode_controller_running_) {
+ control_modes[i].push_back(FORCE_MODE_GPIO);
}
if (passthrough_trajectory_controller_running_) {
- control_modes[i] = PASSTHROUGH_GPIO;
+ control_modes[i].push_back(PASSTHROUGH_GPIO);
}
}
if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(),
- [&](const std::string& other) { return other == start_modes_[0]; })) {
+ [&](const std::vector& other) { return other == start_modes_[0]; })) {
RCLCPP_ERROR(get_logger(), "Start modes of all joints have to be the same.");
return hardware_interface::return_type::ERROR;
}
@@ -910,20 +957,37 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
for (const auto& key : start_interfaces) {
for (auto i = 0u; i < info_.joints.size(); i++) {
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) {
- if (start_modes_[i] != "UNDEFINED") {
+ if (!start_modes_[i].empty()) {
+ RCLCPP_ERROR(get_logger(), "Attempting to start position control while there is another control mode already "
+ "requested.");
return hardware_interface::return_type::ERROR;
}
- start_modes_[i] = hardware_interface::HW_IF_POSITION;
+ start_modes_[i] = { hardware_interface::HW_IF_POSITION };
} else if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) {
- if (start_modes_[i] != "UNDEFINED") {
+ if (!start_modes_[i].empty()) {
+ RCLCPP_ERROR(get_logger(), "Attempting to start velocity control while there is another control mode already "
+ "requested.");
return hardware_interface::return_type::ERROR;
}
- start_modes_[i] = hardware_interface::HW_IF_VELOCITY;
+ start_modes_[i] = { hardware_interface::HW_IF_VELOCITY };
+ } else if (key == tf_prefix + FORCE_MODE_GPIO + "/type") {
+ if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
+ return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY;
+ })) {
+ RCLCPP_ERROR(get_logger(), "Attempting to start force_mode control while there is either position or "
+ "velocity mode already requested by another controller.");
+ return hardware_interface::return_type::ERROR;
+ }
+ start_modes_[i].push_back(FORCE_MODE_GPIO);
} else if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) {
- if (start_modes_[i] != "UNDEFINED") {
+ if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
+ return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY;
+ })) {
+ RCLCPP_ERROR(get_logger(), "Attempting to start trajectory passthrough control while there is either "
+ "position or velocity mode already requested by another controller.");
return hardware_interface::return_type::ERROR;
}
- start_modes_[i] = PASSTHROUGH_GPIO;
+ start_modes_[i].push_back(PASSTHROUGH_GPIO);
}
}
}
@@ -933,56 +997,96 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
for (const auto& key : stop_interfaces) {
for (auto i = 0u; i < info_.joints.size(); i++) {
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) {
- stop_modes_.push_back(StoppingInterface::STOP_POSITION);
- if (control_modes[i] == hardware_interface::HW_IF_POSITION) {
- control_modes[i] = "UNDEFINED";
- }
+ stop_modes_[i].push_back(StoppingInterface::STOP_POSITION);
+ control_modes[i].erase(
+ std::remove_if(control_modes[i].begin(), control_modes[i].end(),
+ [](const std::string& item) { return item == hardware_interface::HW_IF_POSITION; }),
+ control_modes[i].end());
}
if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) {
- stop_modes_.push_back(StoppingInterface::STOP_VELOCITY);
- if (control_modes[i] == hardware_interface::HW_IF_VELOCITY) {
- control_modes[i] = "UNDEFINED";
- }
+ stop_modes_[i].push_back(StoppingInterface::STOP_VELOCITY);
+ control_modes[i].erase(
+ std::remove_if(control_modes[i].begin(), control_modes[i].end(),
+ [](const std::string& item) { return item == hardware_interface::HW_IF_VELOCITY; }),
+ control_modes[i].end());
+ }
+ if (key == tf_prefix + FORCE_MODE_GPIO + "/disable_cmd") {
+ stop_modes_[i].push_back(StoppingInterface::STOP_FORCE_MODE);
+ control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(),
+ [&](const std::string& item) { return item == FORCE_MODE_GPIO; }),
+ control_modes[i].end());
}
if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) {
- stop_modes_.push_back(StoppingInterface::STOP_PASSTHROUGH);
- if (control_modes[i] == PASSTHROUGH_GPIO) {
- control_modes[i] = "UNDEFINED";
- }
+ stop_modes_[i].push_back(StoppingInterface::STOP_PASSTHROUGH);
+ control_modes[i].erase(std::remove_if(control_modes[i].begin(), control_modes[i].end(),
+ [&](const std::string& item) { return item == PASSTHROUGH_GPIO; }),
+ control_modes[i].end());
}
}
}
// Do not start conflicting controllers
- if (std::any_of(start_modes_.begin(), start_modes_.end(),
+ if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
[this](auto& item) { return (item == PASSTHROUGH_GPIO); }) &&
- (std::any_of(start_modes_.begin(), start_modes_.end(),
+ (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
+ [](auto& item) {
+ return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION);
+ }) ||
+ std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
+ return (item == hardware_interface::HW_IF_VELOCITY ||
+ item == hardware_interface::HW_IF_POSITION /*|| item == PASSTHROUGH_GPIO*/);
+ }))) {
+ RCLCPP_ERROR(get_logger(), "Attempting to start passthrough_trajectory control while there is either position or "
+ "velocity mode is running.");
+ ret_val = hardware_interface::return_type::ERROR;
+ }
+
+ if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
+ [this](auto& item) { return (item == FORCE_MODE_GPIO); }) &&
+ (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
[](auto& item) {
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION);
}) ||
- std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) {
+ std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
- item == PASSTHROUGH_GPIO);
+ item == FORCE_MODE_GPIO);
}))) {
+ RCLCPP_ERROR(get_logger(), "Attempting to start force mode control while there is either position or "
+ "velocity mode running.");
ret_val = hardware_interface::return_type::ERROR;
}
- if (std::any_of(start_modes_.begin(), start_modes_.end(),
+
+ // Position mode requested to start
+ if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
[](auto& item) { return (item == hardware_interface::HW_IF_POSITION); }) &&
- (std::any_of(
- start_modes_.begin(), start_modes_.end(),
- [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO); }) ||
- std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) {
+ (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
+ [this](auto& item) {
+ return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO ||
+ item == FORCE_MODE_GPIO);
+ }) ||
+ std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
- item == PASSTHROUGH_GPIO);
+ item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO);
}))) {
+ RCLCPP_ERROR(get_logger(), "Attempting to start position control while there is either trajectory passthrough or "
+ "velocity mode or force_mode running.");
ret_val = hardware_interface::return_type::ERROR;
}
- if (std::any_of(start_modes_.begin(), start_modes_.end(),
+
+ // Velocity mode requested to start
+ if (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
[](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY); }) &&
- std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) {
- return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
- item == PASSTHROUGH_GPIO);
- })) {
+ (std::any_of(start_modes_[0].begin(), start_modes_[0].end(),
+ [this](auto& item) {
+ return (item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO ||
+ item == FORCE_MODE_GPIO);
+ }) ||
+ std::any_of(control_modes[0].begin(), control_modes[0].end(), [this](auto& item) {
+ return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION ||
+ item == PASSTHROUGH_GPIO || item == FORCE_MODE_GPIO);
+ }))) {
+ RCLCPP_ERROR(get_logger(), "Attempting to start velosity control while there is either trajectory passthrough or "
+ "position mode or force_mode running.");
ret_val = hardware_interface::return_type::ERROR;
}
@@ -995,16 +1099,20 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod
{
hardware_interface::return_type ret_val = hardware_interface::return_type::OK;
- if (stop_modes_.size() != 0 &&
- std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_POSITION) != stop_modes_.end()) {
+ if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(),
+ StoppingInterface::STOP_POSITION) != stop_modes_[0].end()) {
position_controller_running_ = false;
urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_;
- } else if (stop_modes_.size() != 0 &&
- std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_VELOCITY) != stop_modes_.end()) {
+ } else if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(),
+ StoppingInterface::STOP_VELOCITY) != stop_modes_[0].end()) {
velocity_controller_running_ = false;
urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
- } else if (stop_modes_.size() != 0 && std::find(stop_modes_.begin(), stop_modes_.end(),
- StoppingInterface::STOP_PASSTHROUGH) != stop_modes_.end()) {
+ } else if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(),
+ StoppingInterface::STOP_FORCE_MODE) != stop_modes_[0].end()) {
+ force_mode_controller_running_ = false;
+ stop_force_mode();
+ } else if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(),
+ StoppingInterface::STOP_PASSTHROUGH) != stop_modes_[0].end()) {
passthrough_trajectory_controller_running_ = false;
passthrough_trajectory_abort_ = 1.0;
trajectory_joint_positions_.clear();
@@ -1012,21 +1120,24 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod
trajectory_joint_velocities_.clear();
}
- if (start_modes_.size() != 0 &&
- std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_POSITION) != start_modes_.end()) {
+ if (start_modes_.size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(),
+ hardware_interface::HW_IF_POSITION) != start_modes_[0].end()) {
velocity_controller_running_ = false;
passthrough_trajectory_controller_running_ = false;
urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_;
position_controller_running_ = true;
- } else if (start_modes_.size() != 0 && std::find(start_modes_.begin(), start_modes_.end(),
- hardware_interface::HW_IF_VELOCITY) != start_modes_.end()) {
+ } else if (start_modes_[0].size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(),
+ hardware_interface::HW_IF_VELOCITY) != start_modes_[0].end()) {
position_controller_running_ = false;
passthrough_trajectory_controller_running_ = false;
urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
velocity_controller_running_ = true;
- } else if (start_modes_.size() != 0 &&
- std::find(start_modes_.begin(), start_modes_.end(), PASSTHROUGH_GPIO) != start_modes_.end()) {
+ } else if (start_modes_[0].size() != 0 &&
+ std::find(start_modes_[0].begin(), start_modes_[0].end(), FORCE_MODE_GPIO) != start_modes_[0].end()) {
+ force_mode_controller_running_ = true;
+ } else if (start_modes_[0].size() != 0 &&
+ std::find(start_modes_[0].begin(), start_modes_[0].end(), PASSTHROUGH_GPIO) != start_modes_[0].end()) {
velocity_controller_running_ = false;
position_controller_running_ = false;
passthrough_trajectory_controller_running_ = true;
@@ -1039,6 +1150,46 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod
return ret_val;
}
+void URPositionHardwareInterface::start_force_mode()
+{
+ for (size_t i = 0; i < force_mode_selection_vector_.size(); i++) {
+ force_mode_selection_vector_copy_[i] = force_mode_selection_vector_[i];
+ }
+ /* Check version of robot to ensure that the correct startForceMode is called. */
+ if (ur_driver_->getVersion().major < 5) {
+ force_mode_async_success_ =
+ ur_driver_->startForceMode(force_mode_task_frame_, force_mode_selection_vector_copy_, force_mode_wrench_,
+ force_mode_type_, force_mode_limits_, force_mode_damping_);
+ if (force_mode_gain_scaling_ != 0.5) {
+ RCLCPP_WARN(rclcpp::get_logger("URPositionHardwareInterface"), "Force mode gain scaling cannot be used on "
+ "CB3 "
+ "robots. Starting force mode, but "
+ "disregarding "
+ "gain scaling.");
+ }
+ } else {
+ force_mode_async_success_ =
+ ur_driver_->startForceMode(force_mode_task_frame_, force_mode_selection_vector_copy_, force_mode_wrench_,
+ force_mode_type_, force_mode_limits_, force_mode_damping_, force_mode_gain_scaling_);
+ }
+
+ for (size_t i = 0; i < 6; i++) {
+ force_mode_task_frame_[i] = NO_NEW_CMD_;
+ force_mode_selection_vector_[i] = static_cast(NO_NEW_CMD_);
+ force_mode_wrench_[i] = NO_NEW_CMD_;
+ force_mode_limits_[i] = NO_NEW_CMD_;
+ }
+ force_mode_type_ = static_cast(NO_NEW_CMD_);
+ force_mode_damping_ = NO_NEW_CMD_;
+ force_mode_gain_scaling_ = NO_NEW_CMD_;
+}
+
+void URPositionHardwareInterface::stop_force_mode()
+{
+ force_mode_async_success_ = ur_driver_->endForceMode();
+ force_mode_disable_cmd_ = NO_NEW_CMD_;
+}
+
void URPositionHardwareInterface::check_passthrough_trajectory_controller()
{
static double last_time = 0.0;
diff --git a/ur_robot_driver/test/integration_test_controller_switch.py b/ur_robot_driver/test/integration_test_controller_switch.py
index 20ea69190..bdc9a0eca 100644
--- a/ur_robot_driver/test/integration_test_controller_switch.py
+++ b/ur_robot_driver/test/integration_test_controller_switch.py
@@ -52,7 +52,8 @@
@pytest.mark.launch_test
@launch_testing.parametrize(
"tf_prefix",
- [(""), ("my_ur_")],
+ [("")],
+ # [(""), ("my_ur_")],
)
def generate_test_description(tf_prefix):
return generate_driver_test_description(tf_prefix=tf_prefix)
@@ -94,6 +95,7 @@ def test_activating_multiple_controllers_same_interface_fails(self):
"forward_position_controller",
"forward_velocity_controller",
"passthrough_trajectory_controller",
+ "force_mode_controller",
],
).ok
)
@@ -117,15 +119,6 @@ def test_activating_multiple_controllers_same_interface_fails(self):
],
).ok
)
- self.assertFalse(
- self._controller_manager_interface.switch_controller(
- strictness=SwitchController.Request.STRICT,
- activate_controllers=[
- "scaled_joint_trajectory_controller",
- "forward_position_controller",
- ],
- ).ok
- )
def test_activating_multiple_controllers_different_interface_fails(self):
# Deactivate all writing controllers
@@ -137,6 +130,7 @@ def test_activating_multiple_controllers_different_interface_fails(self):
"joint_trajectory_controller",
"forward_position_controller",
"forward_velocity_controller",
+ "force_mode_controller",
"passthrough_trajectory_controller",
],
).ok
@@ -177,6 +171,15 @@ def test_activating_multiple_controllers_different_interface_fails(self):
],
).ok
)
+ self.assertFalse(
+ self._controller_manager_interface.switch_controller(
+ strictness=SwitchController.Request.STRICT,
+ activate_controllers=[
+ "scaled_joint_trajectory_controller",
+ "force_mode_controller",
+ ],
+ ).ok
+ )
def test_activating_controller_with_running_position_controller_fails(self):
# Having a position-based controller active, no other controller should be able to
@@ -191,6 +194,7 @@ def test_activating_controller_with_running_position_controller_fails(self):
"joint_trajectory_controller",
"forward_position_controller",
"forward_velocity_controller",
+ "force_mode_controller",
"passthrough_trajectory_controller",
],
).ok
@@ -219,6 +223,14 @@ def test_activating_controller_with_running_position_controller_fails(self):
],
).ok
)
+ self.assertFalse(
+ self._controller_manager_interface.switch_controller(
+ strictness=SwitchController.Request.STRICT,
+ activate_controllers=[
+ "force_mode_controller",
+ ],
+ ).ok
+ )
# Stop controller again
self.assertTrue(
self._controller_manager_interface.switch_controller(
@@ -241,6 +253,7 @@ def test_activating_controller_with_running_passthrough_trajectory_controller_fa
"joint_trajectory_controller",
"forward_position_controller",
"forward_velocity_controller",
+ "force_mode_controller", # tested in separate test
],
).ok
)
@@ -285,3 +298,82 @@ def test_activating_controller_with_running_passthrough_trajectory_controller_fa
],
).ok
)
+
+ def test_force_mode_and_trajectory_passthrough_controller_are_compatible(self):
+ # Deactivate all writing controllers
+ self.assertTrue(
+ self._controller_manager_interface.switch_controller(
+ strictness=SwitchController.Request.BEST_EFFORT,
+ deactivate_controllers=[
+ "scaled_joint_trajectory_controller",
+ "joint_trajectory_controller",
+ "forward_position_controller",
+ "forward_velocity_controller",
+ "passthrough_trajectory_controller",
+ "force_mode_controller",
+ ],
+ ).ok
+ )
+
+ time.sleep(3)
+
+ # Start both together
+ self.assertTrue(
+ self._controller_manager_interface.switch_controller(
+ strictness=SwitchController.Request.STRICT,
+ activate_controllers=[
+ "passthrough_trajectory_controller",
+ "force_mode_controller",
+ ],
+ ).ok
+ )
+
+ # With passthrough traj controller running, start force mode controller
+ self.assertTrue(
+ self._controller_manager_interface.switch_controller(
+ strictness=SwitchController.Request.BEST_EFFORT,
+ activate_controllers=[
+ "passthrough_trajectory_controller",
+ ],
+ deactivate_controllers=[
+ "scaled_joint_trajectory_controller",
+ "joint_trajectory_controller",
+ "forward_position_controller",
+ "forward_velocity_controller",
+ "force_mode_controller",
+ ],
+ ).ok
+ )
+ self.assertTrue(
+ self._controller_manager_interface.switch_controller(
+ strictness=SwitchController.Request.STRICT,
+ activate_controllers=[
+ "force_mode_controller",
+ ],
+ ).ok
+ )
+
+ # With start force mode controller running, passthrough traj controller
+ self.assertTrue(
+ self._controller_manager_interface.switch_controller(
+ strictness=SwitchController.Request.BEST_EFFORT,
+ activate_controllers=[
+ "force_mode_controller",
+ ],
+ deactivate_controllers=[
+ "scaled_joint_trajectory_controller",
+ "joint_trajectory_controller",
+ "forward_position_controller",
+ "forward_velocity_controller",
+ "passthrough_trajectory_controller",
+ ],
+ ).ok
+ )
+ self.assertTrue(
+ self._controller_manager_interface.switch_controller(
+ strictness=SwitchController.Request.STRICT,
+ activate_controllers=[
+ "passthrough_trajectory_controller",
+ ],
+ ).ok
+ )
diff --git a/ur_robot_driver/test/integration_test_force_mode.py b/ur_robot_driver/test/integration_test_force_mode.py
new file mode 100644
index 000000000..3d8c38261
--- /dev/null
+++ b/ur_robot_driver/test/integration_test_force_mode.py
@@ -0,0 +1,524 @@
+#!/usr/bin/env python
+# Copyright 2019, Universal Robots A/S
+#
+# 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.
+
+import os
+import sys
+import time
+import unittest
+
+import pytest
+
+import launch_testing
+import rclpy
+from rclpy.node import Node
+
+from tf2_ros import TransformException
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+
+import std_msgs
+from controller_manager_msgs.srv import SwitchController
+from geometry_msgs.msg import (
+ Pose,
+ PoseStamped,
+ Quaternion,
+ Point,
+ Twist,
+ Wrench,
+ Vector3,
+)
+
+sys.path.append(os.path.dirname(__file__))
+from test_common import ( # noqa: E402
+ ControllerManagerInterface,
+ DashboardInterface,
+ ForceModeInterface,
+ IoStatusInterface,
+ ConfigurationInterface,
+ generate_driver_test_description,
+)
+
+TIMEOUT_EXECUTE_TRAJECTORY = 30
+
+
+@pytest.mark.launch_test
+@launch_testing.parametrize(
+ "tf_prefix",
+ [(""), ("my_ur_")],
+)
+def generate_test_description(tf_prefix):
+ return generate_driver_test_description(tf_prefix=tf_prefix)
+
+
+class RobotDriverTest(unittest.TestCase):
+ @classmethod
+ def setUpClass(cls):
+ # Initialize the ROS context
+ rclpy.init()
+ cls.node = Node("robot_driver_test")
+ time.sleep(1)
+ cls.init_robot(cls)
+
+ @classmethod
+ def tearDownClass(cls):
+ # Shutdown the ROS context
+ cls.node.destroy_node()
+ rclpy.shutdown()
+
+ def init_robot(self):
+ self._dashboard_interface = DashboardInterface(self.node)
+ self._controller_manager_interface = ControllerManagerInterface(self.node)
+ self._io_status_controller_interface = IoStatusInterface(self.node)
+ self._configuration_controller_interface = ConfigurationInterface(self.node)
+
+ def setUp(self):
+ self._dashboard_interface.start_robot()
+ time.sleep(1)
+ self.assertTrue(self._io_status_controller_interface.resend_robot_program().success)
+ self.tf_buffer = Buffer()
+ self.tf_listener = TransformListener(self.tf_buffer, self.node)
+
+ def lookup_tcp_in_base(self, tf_prefix, timepoint):
+ trans = None
+ while not trans:
+ rclpy.spin_once(self.node)
+ try:
+ trans = self.tf_buffer.lookup_transform(
+ tf_prefix + "base", tf_prefix + "tool0", timepoint
+ )
+ except TransformException:
+ pass
+ return trans
+
+ def test_force_mode_controller(self, tf_prefix):
+ self.assertTrue(
+ self._controller_manager_interface.switch_controller(
+ strictness=SwitchController.Request.BEST_EFFORT,
+ activate_controllers=[
+ "force_mode_controller",
+ ],
+ deactivate_controllers=[
+ "scaled_joint_trajectory_controller",
+ "joint_trajectory_controller",
+ ],
+ ).ok
+ )
+ self._force_mode_controller_interface = ForceModeInterface(self.node)
+
+ # Create task frame for force mode
+ point = Point(x=0.8, y=0.8, z=0.8)
+ orientation = Quaternion(x=0.7071, y=0.0, z=0.0, w=0.7071)
+ task_frame_pose = Pose()
+ task_frame_pose.position = point
+ task_frame_pose.orientation = orientation
+ header = std_msgs.msg.Header(seq=1, frame_id=tf_prefix + "base")
+ header.stamp.sec = int(time.time()) + 1
+ header.stamp.nanosec = 0
+ frame_stamp = PoseStamped()
+ frame_stamp.header = header
+ frame_stamp.pose = task_frame_pose
+
+ # Create compliance vector (which axes should be force controlled)
+ compliance = [False, False, True, False, False, False]
+
+ # Create Wrench message for force mode
+ wrench = Wrench()
+ wrench.force = Vector3(x=0.0, y=0.0, z=5.0)
+ wrench.torque = Vector3(x=0.0, y=0.0, z=0.0)
+
+ # Specify interpretation of task frame (no transform)
+ type_spec = 2
+
+ # Specify max speeds and deviations of force mode
+ speed_limits = Twist()
+ speed_limits.linear = Vector3(x=0.0, y=0.0, z=1.0)
+ speed_limits.angular = Vector3(x=0.0, y=0.0, z=1.0)
+ deviation_limits = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+
+ # specify damping and gain scaling
+ damping_factor = 0.1
+ gain_scale = 0.8
+
+ trans_before = self.lookup_tcp_in_base(tf_prefix, rclpy.time.Time())
+
+ # Send request to controller
+ res = self._force_mode_controller_interface.start_force_mode(
+ task_frame=frame_stamp,
+ selection_vector_x=compliance[0],
+ selection_vector_y=compliance[1],
+ selection_vector_z=compliance[2],
+ selection_vector_rx=compliance[3],
+ selection_vector_ry=compliance[4],
+ selection_vector_rz=compliance[5],
+ wrench=wrench,
+ type=type_spec,
+ speed_limits=speed_limits,
+ deviation_limits=deviation_limits,
+ damping_factor=damping_factor,
+ gain_scaling=gain_scale,
+ )
+ self.assertTrue(res.success)
+
+ time.sleep(5.0)
+
+ trans_after = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now())
+
+ # task frame and wrench determines the expected motion
+ # In the example we used
+ # - a task frame rotated pi/2 deg around the base frame's x axis
+ # - a wrench with a positive z component for the force
+ # => we should expect a motion in negative y of the base frame
+ self.assertTrue(trans_after.transform.translation.y < trans_before.transform.translation.y)
+ self.assertAlmostEqual(
+ trans_after.transform.translation.x,
+ trans_before.transform.translation.x,
+ delta=0.001,
+ )
+ self.assertAlmostEqual(
+ trans_after.transform.translation.z,
+ trans_before.transform.translation.z,
+ delta=0.001,
+ )
+ self.assertAlmostEqual(
+ trans_after.transform.rotation.x,
+ trans_before.transform.rotation.x,
+ delta=0.01,
+ )
+ self.assertAlmostEqual(
+ trans_after.transform.rotation.y,
+ trans_before.transform.rotation.y,
+ delta=0.01,
+ )
+ self.assertAlmostEqual(
+ trans_after.transform.rotation.z,
+ trans_before.transform.rotation.z,
+ delta=0.01,
+ )
+ self.assertAlmostEqual(
+ trans_after.transform.rotation.w,
+ trans_before.transform.rotation.w,
+ delta=0.01,
+ )
+
+ res = self._force_mode_controller_interface.stop_force_mode()
+ self.assertTrue(res.success)
+
+ # Deactivate controller
+ self.assertTrue(
+ self._controller_manager_interface.switch_controller(
+ strictness=SwitchController.Request.STRICT,
+ deactivate_controllers=["force_mode_controller"],
+ ).ok
+ )
+
+ def test_illegal_force_mode_types(self, tf_prefix):
+ self.assertTrue(
+ self._controller_manager_interface.switch_controller(
+ strictness=SwitchController.Request.BEST_EFFORT,
+ activate_controllers=[
+ "force_mode_controller",
+ ],
+ deactivate_controllers=[
+ "scaled_joint_trajectory_controller",
+ "joint_trajectory_controller",
+ ],
+ ).ok
+ )
+ self._force_mode_controller_interface = ForceModeInterface(self.node)
+
+ # Create task frame for force mode
+ point = Point(x=0.0, y=0.0, z=0.0)
+ orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
+ task_frame_pose = Pose()
+ task_frame_pose.position = point
+ task_frame_pose.orientation = orientation
+ header = std_msgs.msg.Header(seq=1, frame_id=tf_prefix + "base")
+ header.stamp.sec = int(time.time()) + 1
+ header.stamp.nanosec = 0
+ frame_stamp = PoseStamped()
+ frame_stamp.header = header
+ frame_stamp.pose = task_frame_pose
+
+ res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=0)
+ self.assertFalse(res.success)
+ res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=4)
+ self.assertFalse(res.success)
+ res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=1)
+ self.assertTrue(res.success)
+ res = self._force_mode_controller_interface.stop_force_mode()
+ res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=2)
+ self.assertTrue(res.success)
+ res = self._force_mode_controller_interface.stop_force_mode()
+ res = self._force_mode_controller_interface.start_force_mode(task_frame=frame_stamp, type=3)
+ self.assertTrue(res.success)
+ res = self._force_mode_controller_interface.stop_force_mode()
+
+ def test_illegal_task_frame(self, tf_prefix):
+ self.assertTrue(
+ self._controller_manager_interface.switch_controller(
+ strictness=SwitchController.Request.BEST_EFFORT,
+ activate_controllers=[
+ "force_mode_controller",
+ ],
+ deactivate_controllers=[
+ "scaled_joint_trajectory_controller",
+ "joint_trajectory_controller",
+ ],
+ ).ok
+ )
+ self._force_mode_controller_interface = ForceModeInterface(self.node)
+
+ # Create task frame for force mode
+ point = Point(x=0.0, y=0.0, z=0.0)
+ orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
+ task_frame_pose = Pose()
+ task_frame_pose.position = point
+ task_frame_pose.orientation = orientation
+ header = std_msgs.msg.Header(seq=1, frame_id=tf_prefix + "base")
+ header.stamp.sec = int(time.time()) + 1
+ header.stamp.nanosec = 0
+ frame_stamp = PoseStamped()
+ frame_stamp.header = header
+ frame_stamp.pose = task_frame_pose
+
+ # Illegal frame name produces error
+ header.frame_id = "nonexisting6t54"
+ res = self._force_mode_controller_interface.start_force_mode(
+ task_frame=frame_stamp,
+ )
+ self.assertFalse(res.success)
+ header.frame_id = "base"
+
+ # Illegal quaternion produces error
+ task_frame_pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=0.0)
+ res = self._force_mode_controller_interface.start_force_mode(
+ task_frame=frame_stamp,
+ )
+ self.assertFalse(res.success)
+
+ def test_start_force_mode_on_inactive_controller_fails(self, tf_prefix):
+ self.assertTrue(
+ self._controller_manager_interface.switch_controller(
+ strictness=SwitchController.Request.BEST_EFFORT,
+ activate_controllers=[],
+ deactivate_controllers=[
+ "force_mode_controller",
+ "scaled_joint_trajectory_controller",
+ "joint_trajectory_controller",
+ ],
+ ).ok
+ )
+ self._force_mode_controller_interface = ForceModeInterface(self.node)
+
+ # Create task frame for force mode
+ point = Point(x=0.0, y=0.0, z=0.0)
+ orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
+ task_frame_pose = Pose()
+ task_frame_pose.position = point
+ task_frame_pose.orientation = orientation
+ header = std_msgs.msg.Header(seq=1, frame_id=tf_prefix + "base")
+ header.stamp.sec = int(time.time()) + 1
+ header.stamp.nanosec = 0
+ frame_stamp = PoseStamped()
+ frame_stamp.header = header
+ frame_stamp.pose = task_frame_pose
+
+ res = self._force_mode_controller_interface.start_force_mode(
+ task_frame=frame_stamp,
+ )
+ self.assertFalse(res.success)
+
+ def test_deactivating_controller_stops_force_mode(self, tf_prefix):
+ self.assertTrue(
+ self._controller_manager_interface.switch_controller(
+ strictness=SwitchController.Request.BEST_EFFORT,
+ activate_controllers=[
+ "force_mode_controller",
+ ],
+ deactivate_controllers=[
+ "scaled_joint_trajectory_controller",
+ "joint_trajectory_controller",
+ ],
+ ).ok
+ )
+ self._force_mode_controller_interface = ForceModeInterface(self.node)
+
+ # Create task frame for force mode
+ point = Point(x=0.0, y=0.0, z=0.0)
+ orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
+ task_frame_pose = Pose()
+ task_frame_pose.position = point
+ task_frame_pose.orientation = orientation
+ header = std_msgs.msg.Header(seq=1, frame_id=tf_prefix + "base")
+ header.stamp.sec = int(time.time()) + 1
+ header.stamp.nanosec = 0
+ frame_stamp = PoseStamped()
+ frame_stamp.header = header
+ frame_stamp.pose = task_frame_pose
+
+ # Create compliance vector (which axes should be force controlled)
+ compliance = [False, False, True, False, False, False]
+
+ # Create Wrench message for force mode
+ wrench = Wrench()
+ wrench.force = Vector3(x=0.0, y=0.0, z=5.0)
+ wrench.torque = Vector3(x=0.0, y=0.0, z=0.0)
+
+ # Specify interpretation of task frame (no transform)
+ type_spec = 2
+
+ # Specify max speeds and deviations of force mode
+ speed_limits = Twist()
+ speed_limits.linear = Vector3(x=0.0, y=0.0, z=1.0)
+ speed_limits.angular = Vector3(x=0.0, y=0.0, z=1.0)
+ deviation_limits = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+
+ # specify damping and gain scaling
+ damping_factor = 0.1
+ gain_scale = 0.8
+
+ res = self._force_mode_controller_interface.start_force_mode(
+ task_frame=frame_stamp,
+ selection_vector_x=compliance[0],
+ selection_vector_y=compliance[1],
+ selection_vector_z=compliance[2],
+ selection_vector_rx=compliance[3],
+ selection_vector_ry=compliance[4],
+ selection_vector_rz=compliance[5],
+ wrench=wrench,
+ type=type_spec,
+ speed_limits=speed_limits,
+ deviation_limits=deviation_limits,
+ damping_factor=damping_factor,
+ gain_scaling=gain_scale,
+ )
+ self.assertTrue(res.success)
+
+ time.sleep(0.5)
+
+ self.assertTrue(
+ self._controller_manager_interface.switch_controller(
+ strictness=SwitchController.Request.BEST_EFFORT,
+ activate_controllers=[],
+ deactivate_controllers=[
+ "force_mode_controller",
+ "scaled_joint_trajectory_controller",
+ "joint_trajectory_controller",
+ ],
+ ).ok
+ )
+ self._force_mode_controller_interface = ForceModeInterface(self.node)
+
+ time.sleep(0.5)
+ trans_before_wait = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now())
+
+ # Make sure the robot didn't move anymore
+ time.sleep(0.5)
+ trans_after_wait = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now())
+
+ self.assertAlmostEqual(
+ trans_before_wait.transform.translation.z, trans_after_wait.transform.translation.z
+ )
+
+ def test_params_out_of_range_fails(self, tf_prefix):
+ self.assertTrue(
+ self._controller_manager_interface.switch_controller(
+ strictness=SwitchController.Request.BEST_EFFORT,
+ activate_controllers=[
+ "force_mode_controller",
+ ],
+ deactivate_controllers=[
+ "scaled_joint_trajectory_controller",
+ "joint_trajectory_controller",
+ ],
+ ).ok
+ )
+ self._force_mode_controller_interface = ForceModeInterface(self.node)
+
+ # Create task frame for force mode
+ point = Point(x=0.0, y=0.0, z=0.0)
+ orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
+ task_frame_pose = Pose()
+ task_frame_pose.position = point
+ task_frame_pose.orientation = orientation
+ header = std_msgs.msg.Header(seq=1, frame_id=tf_prefix + "base")
+ header.stamp.sec = int(time.time()) + 1
+ header.stamp.nanosec = 0
+ frame_stamp = PoseStamped()
+ frame_stamp.header = header
+ frame_stamp.pose = task_frame_pose
+
+ res = self._force_mode_controller_interface.start_force_mode(
+ task_frame=frame_stamp, gain_scaling=-0.1
+ )
+ self.assertFalse(res.success)
+
+ res = self._force_mode_controller_interface.start_force_mode(
+ task_frame=frame_stamp, gain_scaling=0
+ )
+ self.assertTrue(res.success)
+ res = self._force_mode_controller_interface.stop_force_mode()
+ self.assertTrue(res.success)
+
+ res = self._force_mode_controller_interface.start_force_mode(
+ task_frame=frame_stamp, gain_scaling=2
+ )
+ self.assertTrue(res.success)
+ res = self._force_mode_controller_interface.stop_force_mode()
+ self.assertTrue(res.success)
+
+ res = self._force_mode_controller_interface.start_force_mode(
+ task_frame=frame_stamp, gain_scaling=2.1
+ )
+ self.assertFalse(res.success)
+
+ # damping factor
+ res = self._force_mode_controller_interface.start_force_mode(
+ task_frame=frame_stamp, damping_factor=-0.1
+ )
+ self.assertFalse(res.success)
+
+ res = self._force_mode_controller_interface.start_force_mode(
+ task_frame=frame_stamp, damping_factor=0
+ )
+ self.assertTrue(res.success)
+ res = self._force_mode_controller_interface.stop_force_mode()
+ self.assertTrue(res.success)
+
+ res = self._force_mode_controller_interface.start_force_mode(
+ task_frame=frame_stamp, damping_factor=1
+ )
+ self.assertTrue(res.success)
+ res = self._force_mode_controller_interface.stop_force_mode()
+ self.assertTrue(res.success)
+
+ res = self._force_mode_controller_interface.start_force_mode(
+ task_frame=frame_stamp, damping_factor=1.1
+ )
+ self.assertFalse(res.success)
diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py
index 6350ea78d..b0b2a4c84 100755
--- a/ur_robot_driver/test/robot_driver.py
+++ b/ur_robot_driver/test/robot_driver.py
@@ -52,19 +52,11 @@
IoStatusInterface,
ConfigurationInterface,
generate_driver_test_description,
+ ROBOT_JOINTS,
)
TIMEOUT_EXECUTE_TRAJECTORY = 30
-ROBOT_JOINTS = [
- "shoulder_pan_joint",
- "shoulder_lift_joint",
- "elbow_joint",
- "wrist_1_joint",
- "wrist_2_joint",
- "wrist_3_joint",
-]
-
@pytest.mark.launch_test
@launch_testing.parametrize(
diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py
index a8a1a8ffb..927542676 100644
--- a/ur_robot_driver/test/test_common.py
+++ b/ur_robot_driver/test/test_common.py
@@ -57,7 +57,7 @@
IsProgramRunning,
Load,
)
-from ur_msgs.srv import SetIO, GetRobotSoftwareVersion
+from ur_msgs.srv import SetIO, GetRobotSoftwareVersion, SetForceMode
TIMEOUT_WAIT_SERVICE = 10
TIMEOUT_WAIT_SERVICE_INITIAL = 120 # If we download the docker image simultaneously to the tests, it can take quite some time until the dashboard server is reachable and usable.
@@ -274,6 +274,15 @@ class ConfigurationInterface(
pass
+class ForceModeInterface(
+ _ServiceInterface,
+ namespace="/force_mode_controller",
+ initial_services={},
+ services={"start_force_mode": SetForceMode, "stop_force_mode": Trigger},
+):
+ pass
+
+
def _declare_launch_arguments():
declared_arguments = []
diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro
index d5df9b2b8..35b0a8cb9 100644
--- a/ur_robot_driver/urdf/ur.ros2_control.xacro
+++ b/ur_robot_driver/urdf/ur.ros2_control.xacro
@@ -229,6 +229,38 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+