Skip to content

Commit f1863a2

Browse files
URJalaurfeex
URJala
and
urfeex
authored
Added controller to enable and disable tool contact (UniversalRobots#940)
This adds a new controller to enable tool contact mode on the robot. --------- Co-authored-by: Felix Exner <[email protected]>
1 parent 14f80ac commit f1863a2

15 files changed

+1016
-23
lines changed

‎ur_controllers/CMakeLists.txt

+8
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,11 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
5151

5252
include_directories(include)
5353

54+
generate_parameter_library(
55+
tool_contact_controller_parameters
56+
src/tool_contact_controller_parameters.yaml
57+
)
58+
5459
generate_parameter_library(
5560
force_mode_controller_parameters
5661
src/force_mode_controller_parameters.yaml
@@ -87,6 +92,7 @@ generate_parameter_library(
8792
)
8893

8994
add_library(${PROJECT_NAME} SHARED
95+
src/tool_contact_controller.cpp
9096
src/force_mode_controller.cpp
9197
src/scaled_joint_trajectory_controller.cpp
9298
src/speed_scaling_state_broadcaster.cpp
@@ -99,6 +105,7 @@ target_include_directories(${PROJECT_NAME} PRIVATE
99105
include
100106
)
101107
target_link_libraries(${PROJECT_NAME}
108+
tool_contact_controller_parameters
102109
force_mode_controller_parameters
103110
gpio_controller_parameters
104111
speed_scaling_state_broadcaster_parameters
@@ -107,6 +114,7 @@ target_link_libraries(${PROJECT_NAME}
107114
passthrough_trajectory_controller_parameters
108115
ur_configuration_controller_parameters
109116
)
117+
110118
ament_target_dependencies(${PROJECT_NAME}
111119
${THIS_PACKAGE_INCLUDE_DEPENDS}
112120
)

‎ur_controllers/controller_plugins.xml

+5
Original file line numberDiff line numberDiff line change
@@ -34,4 +34,9 @@
3434
Controller used to get and change the configuration of the robot
3535
</description>
3636
</class>
37+
<class name="ur_controllers/ToolContactController" type="ur_controllers::ToolContactController" base_class_type="controller_interface::ControllerInterface">
38+
<description>
39+
Controller to use the tool contact functionality of the robot.
40+
</description>
41+
</class>
3742
</library>

‎ur_controllers/doc/index.rst

+44
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@ robot family. Currently this contains:
1111
but it uses the speed scaling reported to align progress of the trajectory between the robot and controller.
1212
* A **io_and_status_controller** that allows setting I/O ports, controlling some UR-specific
1313
functionality and publishes status information about the robot.
14+
* A **tool_contact_controller** that exposes an action to enable the tool contact function on the robot.
1415

1516
About this package
1617
------------------
@@ -378,3 +379,46 @@ The controller provides the ``~/enable_freedrive_mode`` topic of type ``[std_msg
378379
* to deactivate freedrive mode is enough to publish a ``False`` msg on the indicated topic or
379380
to deactivate the controller or to stop publishing ``True`` on the enable topic and wait for the
380381
controller timeout.
382+
383+
.. _tool_contact_controller:
384+
385+
ur_controllers/ToolContactController
386+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
387+
This controller can enable tool contact on the robot. When tool contact is enabled, and the robot
388+
senses whether the tool has made contact with something. When that happens, it will stop all
389+
motion, and retract to where it first sensed the contact.
390+
391+
This controller can be used with any of the motion controllers.
392+
393+
The controller is not a direct representation of the URScript function `tool_contact(direction)
394+
<https://www.universal-robots.com/manuals/EN/HTML/SW5_21/Content/prod-scriptmanual/all_scripts/tool_contact%28direction%29.htm?Highlight=tool_contact>`_,
395+
as it does not allow for choosing the direction. The direction of tool contact will always be the
396+
current TCP direction of movement.
397+
398+
Parameters
399+
""""""""""
400+
401+
+-------------------------+--------+---------------+---------------------------------------------------------------------------------------+
402+
| Parameter name | Type | Default value | Description |
403+
| | | | |
404+
+-------------------------+--------+---------------+---------------------------------------------------------------------------------------+
405+
| ``tf_prefix`` | string | <empty> | Urdf prefix of the corresponding arm |
406+
+-------------------------+--------+---------------+---------------------------------------------------------------------------------------+
407+
| ``action_monitor_rate`` | double | 20.0 | The rate at which the action should be monitored in Hz. |
408+
+-------------------------+--------+---------------+---------------------------------------------------------------------------------------+
409+
410+
Action interface / usage
411+
""""""""""""""""""""""""
412+
The controller provides one action for enabling tool contact. For the controller to accept action goals it needs to be in ``active`` state.
413+
414+
* ``~/detect_tool_contact [ur_msgs/action/ToolContact]``
415+
416+
The action definition of ``ur_msgs/action/ToolContact`` has no fields, as a call to the action implicitly means that tool contact should be enabled.
417+
The result of the action is available through the status of the action itself. If the action succeeds it means that tool contact was detected, otherwise tool contact will remain active until it is either cancelled by the user, or aborted by the hardware.
418+
The action provides no feedback.
419+
420+
The action can be called from the command line using the following command, when the controller is active:
421+
422+
.. code-block::
423+
424+
ros2 action send_goal /tool_contact_controller/detect_tool_contact ur_msgs/action/ToolContact

‎ur_controllers/include/ur_controllers/tool_contact_controller.hpp

+135
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,135 @@
1+
// Copyright 2025, Universal Robots A/S
2+
//
3+
// Redistribution and use in source and binary forms, with or without
4+
// modification, are permitted provided that the following conditions are met:
5+
//
6+
// * Redistributions of source code must retain the above copyright
7+
// notice, this list of conditions and the following disclaimer.
8+
//
9+
// * Redistributions in binary form must reproduce the above copyright
10+
// notice, this list of conditions and the following disclaimer in the
11+
// documentation and/or other materials provided with the distribution.
12+
//
13+
// * Neither the name of the {copyright_holder} nor the names of its
14+
// contributors may be used to endorse or promote products derived from
15+
// this software without specific prior written permission.
16+
//
17+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27+
// POSSIBILITY OF SUCH DAMAGE.
28+
29+
//----------------------------------------------------------------------
30+
/*!\file
31+
*
32+
* \author Jacob Larsen [email protected]
33+
* \date 2025-01-07
34+
*
35+
*
36+
*
37+
*
38+
*/
39+
//----------------------------------------------------------------------
40+
41+
#ifndef UR_CONTROLLERS__TOOL_CONTACT_CONTROLLER_HPP_
42+
#define UR_CONTROLLERS__TOOL_CONTACT_CONTROLLER_HPP_
43+
44+
#include <chrono>
45+
#include <vector>
46+
#include <memory>
47+
48+
#include <controller_interface/controller_interface.hpp>
49+
#include "std_msgs/msg/bool.hpp"
50+
#include <rclcpp_action/server.hpp>
51+
#include <rclcpp_action/create_server.hpp>
52+
#include <rclcpp/rclcpp.hpp>
53+
#include <rclcpp_action/server_goal_handle.hpp>
54+
#include <rclcpp/duration.hpp>
55+
56+
#include <realtime_tools/realtime_buffer.hpp>
57+
#include <realtime_tools/realtime_server_goal_handle.hpp>
58+
59+
#include <ur_msgs/action/tool_contact.hpp>
60+
#include "ur_controllers/tool_contact_controller_parameters.hpp"
61+
62+
namespace ur_controllers
63+
{
64+
class ToolContactController : public controller_interface::ControllerInterface
65+
{
66+
public:
67+
ToolContactController() = default;
68+
~ToolContactController() override = default;
69+
70+
controller_interface::CallbackReturn on_init() override;
71+
72+
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
73+
74+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
75+
76+
controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
77+
78+
controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
79+
80+
controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
81+
82+
controller_interface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state) override;
83+
84+
controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
85+
86+
private:
87+
using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle<ur_msgs::action::ToolContact>;
88+
using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
89+
using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer<RealtimeGoalHandlePtr>;
90+
91+
RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any.
92+
ur_msgs::action::ToolContact::Feedback::SharedPtr feedback_; ///< preallocated feedback
93+
rclcpp::TimerBase::SharedPtr goal_handle_timer_; ///< Timer to frequently check on the running goal
94+
95+
// non-rt function that will be called with action_monitor_period to monitor the rt action
96+
rclcpp::Duration action_monitor_period_ = rclcpp::Duration::from_seconds(0.05);
97+
void action_handler();
98+
99+
rclcpp_action::GoalResponse goal_received_callback(const rclcpp_action::GoalUUID& /*uuid*/,
100+
std::shared_ptr<const ur_msgs::action::ToolContact::Goal> goal);
101+
102+
void
103+
goal_accepted_callback(std::shared_ptr<rclcpp_action::ServerGoalHandle<ur_msgs::action::ToolContact>> goal_handle);
104+
105+
rclcpp_action::CancelResponse goal_canceled_callback(
106+
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ur_msgs::action::ToolContact>> goal_handle);
107+
108+
std::atomic<bool> tool_contact_enable_ = false;
109+
std::atomic<bool> tool_contact_active_ = false;
110+
std::atomic<bool> tool_contact_abort_ = false;
111+
std::atomic<bool> change_requested_ = false;
112+
std::atomic<bool> logged_once_ = false;
113+
std::atomic<bool> should_reset_goal = false;
114+
115+
std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> tool_contact_result_interface_;
116+
std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> major_version_state_interface_;
117+
std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> tool_contact_state_interface_;
118+
std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> tool_contact_set_state_interface_;
119+
120+
rclcpp_action::Server<ur_msgs::action::ToolContact>::SharedPtr tool_contact_action_server_;
121+
122+
std::shared_ptr<tool_contact_controller::ParamListener> tool_contact_param_listener_;
123+
tool_contact_controller::Params tool_contact_params_;
124+
125+
static constexpr double TOOL_CONTACT_STANDBY = 1.0;
126+
static constexpr double TOOL_CONTACT_WAITING_BEGIN = 2.0;
127+
static constexpr double TOOL_CONTACT_EXECUTING = 3.0;
128+
static constexpr double TOOL_CONTACT_FAILURE_BEGIN = 4.0;
129+
static constexpr double TOOL_CONTACT_WAITING_END = 5.0;
130+
static constexpr double TOOL_CONTACT_SUCCESS_END = 6.0;
131+
static constexpr double TOOL_CONTACT_FAILURE_END = 7.0;
132+
};
133+
} // namespace ur_controllers
134+
135+
#endif // UR_CONTROLLERS__TOOL_CONTACT_CONTROLLER_HPP_

0 commit comments

Comments
 (0)