diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt
index a2b72e599..46ef4536c 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)
@@ -23,6 +26,7 @@ find_package(generate_parameter_library REQUIRED)
set(THIS_PACKAGE_INCLUDE_DEPENDS
angles
controller_interface
+ geometry_msgs
joint_trajectory_controller
lifecycle_msgs
pluginlib
@@ -31,6 +35,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
@@ -38,6 +44,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
@@ -55,6 +65,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)
@@ -63,6 +74,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
diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml
index f0058ab55..917bb0984 100644
--- a/ur_controllers/controller_plugins.xml
+++ b/ur_controllers/controller_plugins.xml
@@ -14,4 +14,9 @@
This controller publishes the Tool IO.
+
+
+ Controller to use UR's force_mode.
+
+
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..28ab6cc18
--- /dev/null
+++ b/ur_controllers/include/ur_controllers/force_mode_controller.hpp
@@ -0,0 +1,124 @@
+// 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 "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,
+};
+
+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;
+
+private:
+ bool setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req,
+ ur_msgs::srv::SetForceMode::Response::SharedPtr resp);
+ bool disableForceMode();
+ rclcpp::Service::SharedPtr set_force_mode_srv_;
+
+ std::unique_ptr tf_buffer_;
+ std::unique_ptr tf_listener_;
+
+ std::shared_ptr param_listener_;
+ force_mode_controller::Params params_;
+
+ 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 3b77510bd..b0020999d 100644
--- a/ur_controllers/package.xml
+++ b/ur_controllers/package.xml
@@ -22,6 +22,7 @@
angles
controller_interface
+ geometry_msgs
joint_trajectory_controller
lifecycle_msgs
pluginlib
@@ -30,6 +31,8 @@
realtime_tools
std_msgs
std_srvs
+ tf2_geometry_msgs
+ tf2_ros
ur_dashboard_msgs
ur_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..04f5b6702
--- /dev/null
+++ b/ur_controllers/src/force_mode_controller.cpp
@@ -0,0 +1,284 @@
+// 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
+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::return_type ur_controllers::ForceModeController::update(const rclcpp::Time& /*time*/,
+ const rclcpp::Duration& /*period*/)
+{
+ // Publish state of force_mode?
+
+ return controller_interface::return_type::OK;
+}
+
+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_);
+
+ return LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn
+ur_controllers::ForceModeController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+ RCLCPP_WARN(get_node()->get_logger(), "The ForceModeController is considered a beta feature. Its interface might "
+ "change in the future.");
+ while (state_interfaces_[StateInterfaces::INITIALIZED_FLAG].get_value() == 0.0) {
+ RCLCPP_INFO(get_node()->get_logger(), "Waiting for system interface to initialize...");
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+ }
+
+ // 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));
+ } catch (...) {
+ return LifecycleNodeInterface::CallbackReturn::ERROR;
+ }
+ 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.
+ disableForceMode();
+ try {
+ set_force_mode_srv_.reset();
+ } catch (...) {
+ return LifecycleNodeInterface::CallbackReturn::ERROR;
+ }
+ return LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request::SharedPtr req,
+ ur_msgs::srv::SetForceMode::Response::SharedPtr resp)
+{
+ // reset success flag
+ command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+
+ // transform task frame into base
+ const std::string tf_prefix = params_.tf_prefix;
+ try {
+ auto task_frame_transformed = tf_buffer_->transform(req->task_frame, tf_prefix + "base");
+ command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_X].set_value(task_frame_transformed.pose.position.x);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Y].set_value(task_frame_transformed.pose.position.y);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Z].set_value(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);
+ std::vector rot(3);
+ rot_mat.getRPY(rot[0], rot[1], rot[2]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RX].set_value(rot[0]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RY].set_value(rot[1]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RZ].set_value(rot[2]);
+ } 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());
+ }
+
+ /* The selection vector dictates which axes the robot should be compliant along and around */
+ command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_X].set_value(req->selection_vector_x);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Y].set_value(req->selection_vector_y);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Z].set_value(req->selection_vector_z);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RX].set_value(req->selection_vector_rx);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RY].set_value(req->selection_vector_ry);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RZ].set_value(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. */
+ command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_X].set_value(req->wrench.wrench.force.x);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Y].set_value(req->wrench.wrench.force.y);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Z].set_value(req->wrench.wrench.force.z);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RX].set_value(req->wrench.wrench.torque.x);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RY].set_value(req->wrench.wrench.torque.y);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RZ].set_value(req->wrench.wrench.torque.z);
+
+ /* 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. */
+ command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X].set_value(req->limits[0]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Y].set_value(req->limits[1]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Z].set_value(req->limits[2]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RX].set_value(req->limits[3]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RY].set_value(req->limits[4]);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RZ].set_value(req->limits[5]);
+
+ /* 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. */
+ command_interfaces_[CommandInterfaces::FORCE_MODE_TYPE].set_value(unsigned(req->type));
+
+ /* The damping factor decides how fast the robot decelarates if no force is present. 0 means no deceleration, 1 means
+ * quick deceleration*/
+ command_interfaces_[CommandInterfaces::FORCE_MODE_DAMPING].set_value(req->damping_factor);
+ /*The gain scaling factor scales the force mode gain. A value larger than 1 may make force mode unstable. */
+ command_interfaces_[CommandInterfaces::FORCE_MODE_GAIN_SCALING].set_value(req->gain_scaling);
+
+ 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 (command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+ retries++;
+
+ if (retries > maximum_retries) {
+ resp->success = false;
+ }
+ }
+
+ resp->success = static_cast(command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value());
+
+ 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()
+{
+ command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+ command_interfaces_[CommandInterfaces::FORCE_MODE_DISABLE_CMD].set_value(1);
+
+ RCLCPP_DEBUG(get_node()->get_logger(), "Waiting for force mode to be disabled.");
+ while (command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value() == ASYNC_WAITING) {
+ // Asynchronous wait until the hardware interface has set the force mode
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+ }
+ bool success = static_cast(command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].get_value());
+ if (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..835606780
--- /dev/null
+++ b/ur_controllers/src/force_mode_controller_parameters.yaml
@@ -0,0 +1,29 @@
+force_mode_controller:
+ damping: {
+ type: double,
+ default_value: 0.025,
+ description: "Sets the damping parameter in force mode. 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.",
+ read_only: true,
+ validation: {
+ bounds<>: [0.0, 1.0]
+ }
+ }
+ gain_scaling: {
+ type: double,
+ default_value: 0.5,
+ description: "Scales the gain in force mode. A value larger than 1 can make force mode unstable, e.g. in case of collisions or pushing against hard surfaces.",
+ read_only: true,
+ validation: {
+ bounds<>: [0.0, 2.0]
+ }
+ }
+ 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/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp
index d5e6d16ee..9549b77f0 100644
--- a/ur_controllers/src/gpio_controller.cpp
+++ b/ur_controllers/src/gpio_controller.cpp
@@ -35,9 +35,9 @@
*/
//----------------------------------------------------------------------
-#include "ur_controllers/gpio_controller.hpp"
+#include
-#include
+#include "ur_controllers/gpio_controller.hpp"
namespace ur_controllers
{
diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt
index d341b911f..c58b50204 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
@@ -147,6 +149,8 @@ install(PROGRAMS
scripts/tool_communication.py
scripts/wait_for_robot_description
scripts/start_ursim.sh
+ examples/examples.py
+ examples/force_mode.py
DESTINATION lib/${PROJECT_NAME}
)
diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml
index a512dc1ca..c139e82d4 100644
--- a/ur_robot_driver/config/ur_controllers.yaml
+++ b/ur_robot_driver/config/ur_controllers.yaml
@@ -24,6 +24,9 @@ controller_manager:
forward_position_controller:
type: position_controllers/JointGroupPositionController
+ force_mode_controller:
+ type: ur_controllers/ForceModeController
+
speed_scaling_state_broadcaster:
ros__parameters:
@@ -124,3 +127,7 @@ forward_position_controller:
- $(var tf_prefix)wrist_1_joint
- $(var tf_prefix)wrist_2_joint
- $(var tf_prefix)wrist_3_joint
+
+force_mode_controller:
+ ros__parameters:
+ tf_prefix: "$(var tf_prefix)"
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 67e27a222..461aa734f 100644
--- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp
+++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp
@@ -197,6 +197,17 @@ 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::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_;
diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py
index 461f15034..76f1442a7 100644
--- a/ur_robot_driver/launch/ur_control.launch.py
+++ b/ur_robot_driver/launch/ur_control.launch.py
@@ -159,7 +159,7 @@ def controller_spawner(controllers, active=True):
"speed_scaling_state_broadcaster",
"force_torque_sensor_broadcaster",
]
- controllers_inactive = ["forward_position_controller"]
+ controllers_inactive = ["forward_position_controller", "force_mode_controller"]
controller_spawners = [controller_spawner(controllers_active)] + [
controller_spawner(controllers_inactive, active=False)
diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp
index 5826cf595..9b4f58779 100644
--- a/ur_robot_driver/src/hardware_interface.cpp
+++ b/ur_robot_driver/src/hardware_interface.cpp
@@ -279,6 +279,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", "task_frame_x", &force_mode_task_frame_[0]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "task_frame_y", &force_mode_task_frame_[1]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "task_frame_z", &force_mode_task_frame_[2]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "task_frame_rx", &force_mode_task_frame_[3]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "task_frame_ry", &force_mode_task_frame_[4]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "task_frame_rz", &force_mode_task_frame_[5]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "selection_vector_x", &force_mode_selection_vector_[0]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "selection_vector_y", &force_mode_selection_vector_[1]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "selection_vector_z", &force_mode_selection_vector_[2]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "selection_vector_rx", &force_mode_selection_vector_[3]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "selection_vector_ry", &force_mode_selection_vector_[4]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "selection_vector_rz", &force_mode_selection_vector_[5]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "wrench_x", &force_mode_wrench_[0]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "wrench_y", &force_mode_wrench_[1]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "wrench_z", &force_mode_wrench_[2]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "wrench_rx", &force_mode_wrench_[3]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "wrench_ry", &force_mode_wrench_[4]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "wrench_rz", &force_mode_wrench_[5]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "type", &force_mode_type_);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "limits_x", &force_mode_limits_[0]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "limits_y", &force_mode_limits_[1]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "limits_z", &force_mode_limits_[2]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "limits_rx", &force_mode_limits_[3]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "limits_ry", &force_mode_limits_[4]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "limits_rz", &force_mode_limits_[5]);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "force_mode_async_success", &force_mode_async_success_);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "disable_cmd", &force_mode_disable_cmd_);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "damping", &force_mode_damping_);
+ command_interfaces.emplace_back(tf_prefix + "force_mode", "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]));
@@ -603,6 +633,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;
}
@@ -660,6 +691,14 @@ void URPositionHardwareInterface::initAsyncIO()
payload_mass_ = NO_NEW_CMD_;
payload_center_of_gravity_ = { NO_NEW_CMD_, NO_NEW_CMD_, NO_NEW_CMD_ };
+
+ 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_);
}
void URPositionHardwareInterface::checkAsyncIO()
@@ -727,6 +766,44 @@ void URPositionHardwareInterface::checkAsyncIO()
zero_ftsensor_async_success_ = ur_driver_->zeroFTSensor();
zero_ftsensor_cmd_ = NO_NEW_CMD_;
}
+ 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) {
+ urcl::vector6uint32_t force_mode_selection_vector;
+ for (size_t i = 0; i < 6; i++) {
+ force_mode_selection_vector[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, 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,
+ 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_;
+ }
+
+ if (!std::isnan(force_mode_disable_cmd_) && ur_driver_ != nullptr && force_mode_async_success_ == 2.0) {
+ force_mode_async_success_ = ur_driver_->endForceMode();
+ force_mode_disable_cmd_ = NO_NEW_CMD_;
+ }
}
void URPositionHardwareInterface::updateNonDoubleValues()
diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py
index 96c859947..a94619ece 100755
--- a/ur_robot_driver/test/robot_driver.py
+++ b/ur_robot_driver/test/robot_driver.py
@@ -35,12 +35,22 @@
import launch_testing
import pytest
import rclpy
+import std_msgs
from builtin_interfaces.msg import Duration
from control_msgs.action import FollowJointTrajectory
from controller_manager_msgs.srv import SwitchController
from rclpy.node import Node
from sensor_msgs.msg import JointState
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from geometry_msgs.msg import (
+ Pose,
+ PoseStamped,
+ Quaternion,
+ Point,
+ WrenchStamped,
+ Wrench,
+ Vector3,
+)
from ur_msgs.msg import IOStates
sys.path.append(os.path.dirname(__file__))
@@ -49,6 +59,7 @@
ControllerManagerInterface,
DashboardInterface,
IoStatusInterface,
+ ForceModeInterface,
generate_driver_test_description,
)
@@ -92,7 +103,6 @@ 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._scaled_follow_joint_trajectory = ActionInterface(
self.node,
"/scaled_joint_trajectory_controller/follow_joint_trajectory",
@@ -100,13 +110,110 @@ def init_robot(self):
)
def setUp(self):
- self._dashboard_interface.start_robot()
+ # self._dashboard_interface.start_robot()
time.sleep(1)
self.assertTrue(self._io_status_controller_interface.resend_robot_program().success)
#
# Test functions
#
+ def test_force_mode_controller(self, tf_prefix):
+ # Activate force mode and trajectory controller, to make sure the robot can be moved.
+ self.assertTrue(
+ self._controller_manager_interface.switch_controller(
+ strictness=SwitchController.Request.BEST_EFFORT,
+ activate_controllers=[
+ "force_mode_controller",
+ "scaled_joint_trajectory_controller",
+ ],
+ ).ok
+ )
+ self._force_mode_controller_interface = ForceModeInterface(self.node)
+
+ # Move the robot away from any singularities created in other tests.
+ # Construct test trajectory
+ test_trajectory = [
+ (Duration(sec=10, nanosec=0), [-1.5 for j in ROBOT_JOINTS]),
+ ]
+
+ trajectory = JointTrajectory(
+ joint_names=[tf_prefix + joint for joint in ROBOT_JOINTS],
+ points=[
+ JointTrajectoryPoint(positions=test_pos, time_from_start=test_time)
+ for (test_time, test_pos) in test_trajectory
+ ],
+ )
+
+ # Sending trajectory goal
+ logging.info("Sending simple goal")
+ goal_handle = self._scaled_follow_joint_trajectory.send_goal(trajectory=trajectory)
+ self.assertTrue(goal_handle.accepted)
+
+ # Verify execution
+ result = self._scaled_follow_joint_trajectory.get_result(
+ goal_handle, TIMEOUT_EXECUTE_TRAJECTORY
+ )
+ self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL)
+ # Finished moving
+
+ # Create task frame for force mode
+ point = Point(x=0.8, y=0.8, z=0.8)
+ orientation = Quaternion(x=0.0, y=0.2, z=0.5, w=0.3)
+ task_frame_pose = Pose()
+ task_frame_pose.position = point
+ task_frame_pose.orientation = orientation
+ header = std_msgs.msg.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, True]
+
+ # Create Wrench message for force mode
+ wrench_vec = Wrench()
+ wrench_vec.force = Vector3(x=1.0, y=2.0, z=3.0)
+ wrench_vec.torque = wrench_vec.force
+ wrench_stamp = WrenchStamped()
+ wrench_stamp.header = header
+ wrench_stamp.wrench = wrench_vec
+
+ # Specify interpretation of task frame (no transform)
+ type_spec = 2
+
+ # Specify max speeds and deviations of force mode
+ limits = [1.0, 1.0, 1.0, 0.5, 1.0, 1.0, 1.0]
+
+ # specify damping and gain scaling
+ damping_factor = 0.1
+ gain_scale = 0.8
+
+ # 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_stamp,
+ type=type_spec,
+ limits=limits,
+ damping_factor=damping_factor,
+ gain_scaling=gain_scale,
+ )
+ self.assertEqual(res.success, True)
+
+ # Deactivate controller to stop force mode
+ self.assertTrue(
+ self._controller_manager_interface.switch_controller(
+ strictness=SwitchController.Request.BEST_EFFORT,
+ deactivate_controllers=["force_mode_controller"],
+ ).ok
+ )
def test_start_scaled_jtc_controller(self):
self.assertTrue(
diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py
index 4a0b2ad1b..f1b970d82 100644
--- a/ur_robot_driver/test/test_common.py
+++ b/ur_robot_driver/test/test_common.py
@@ -52,7 +52,7 @@
IsProgramRunning,
Load,
)
-from ur_msgs.srv import SetIO
+from ur_msgs.srv import SetIO, 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.
@@ -245,6 +245,15 @@ class IoStatusInterface(
pass
+class ForceModeInterface(
+ _ServiceInterface,
+ namespace="/force_mode_controller",
+ initial_services={},
+ services={"start_force_mode": SetForceMode},
+):
+ pass
+
+
def _declare_launch_arguments():
declared_arguments = []