Skip to content

Commit 8e956e7

Browse files
urfeexurfeex
committed
Add twist controller
1 parent 5b0ae48 commit 8e956e7

File tree

7 files changed

+257
-0
lines changed

7 files changed

+257
-0
lines changed

‎ur_controllers/CMakeLists.txt‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -87,6 +87,11 @@ generate_parameter_library(
8787
src/passthrough_trajectory_controller_parameters.yaml
8888
)
8989

90+
generate_parameter_library(
91+
twist_controller_parameters
92+
src/twist_controller_parameters.yaml
93+
)
94+
9095
generate_parameter_library(
9196
ur_configuration_controller_parameters
9297
src/ur_configuration_controller_parameters.yaml
@@ -100,6 +105,7 @@ add_library(${PROJECT_NAME} SHARED
100105
src/freedrive_mode_controller.cpp
101106
src/gpio_controller.cpp
102107
src/passthrough_trajectory_controller.cpp
108+
src/twist_controller.cpp
103109
src/ur_configuration_controller.cpp)
104110

105111
target_include_directories(${PROJECT_NAME} PRIVATE
@@ -114,6 +120,7 @@ target_link_libraries(${PROJECT_NAME}
114120
freedrive_mode_controller_parameters
115121
passthrough_trajectory_controller_parameters
116122
ur_configuration_controller_parameters
123+
twist_controller_parameters
117124
${geometry_msgs_TARGETS}
118125
${lifecycle_msgs_TARGETS}
119126
${std_msgs_TARGETS}

‎ur_controllers/controller_plugins.xml‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -39,4 +39,9 @@
3939
Controller to use the tool contact functionality of the robot.
4040
</description>
4141
</class>
42+
<class name="ur_controllers/TwistController" type="ur_controllers::TwistController" base_class_type="controller_interface::ControllerInterface">
43+
<description>
44+
Controller to move a manipulator's TCP using Cartesian velocities.
45+
</description>
46+
</class>
4247
</library>

‎ur_controllers/include/ur_controllers/twist_controller.hpp‎

Lines changed: 75 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
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+
#ifndef UR_CONTROLLERS__TWIST_CONTROLLER_HPP_
30+
#define UR_CONTROLLERS__TWIST_CONTROLLER_HPP_
31+
32+
#include <memory>
33+
34+
#include <controller_interface/controller_interface.hpp>
35+
#include <realtime_tools/realtime_thread_safe_box.hpp>
36+
#include <geometry_msgs/msg/twist_stamped.hpp>
37+
38+
#include "ur_controllers/twist_controller_parameters.hpp"
39+
40+
namespace ur_controllers
41+
{
42+
class TwistController : public controller_interface::ControllerInterface
43+
{
44+
public:
45+
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
46+
47+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
48+
49+
controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
50+
51+
CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
52+
53+
CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
54+
55+
CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
56+
57+
CallbackReturn on_init() override;
58+
59+
private:
60+
void reset();
61+
62+
using TwistStamped = geometry_msgs::msg::TwistStamped;
63+
64+
bool subscriber_is_active_ = false;
65+
rclcpp::Subscription<TwistStamped>::SharedPtr twist_command_subscriber_ = nullptr;
66+
67+
realtime_tools::RealtimeThreadSafeBox<TwistStamped> received_twist_msg_;
68+
TwistStamped command_msg_;
69+
70+
std::shared_ptr<twist_controller::ParamListener> param_listener_;
71+
twist_controller::Params controller_params_;
72+
};
73+
} // namespace ur_controllers
74+
75+
#endif // UR_CONTROLLERS__TWIST_CONTROLLER_HPP_

‎ur_controllers/src/twist_controller.cpp‎

Lines changed: 155 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,155 @@
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+
#include <ur_controllers/twist_controller.hpp>
30+
31+
namespace ur_controllers
32+
{
33+
controller_interface::CallbackReturn TwistController::on_init()
34+
{
35+
param_listener_ = std::make_shared<twist_controller::ParamListener>(get_node());
36+
controller_params_ = param_listener_->get_params();
37+
38+
return controller_interface::CallbackReturn::SUCCESS;
39+
}
40+
41+
controller_interface::CallbackReturn TwistController::on_configure(const rclcpp_lifecycle::State& /* previous_state */)
42+
{
43+
twist_command_subscriber_ = get_node()->create_subscription<TwistStamped>(
44+
"~/twist", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr<TwistStamped> msg) -> void {
45+
if (!subscriber_is_active_) {
46+
RCLCPP_WARN(get_node()->get_logger(), "Can't accept new commands. subscriber is inactive");
47+
return;
48+
}
49+
if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) {
50+
RCLCPP_WARN_ONCE(get_node()->get_logger(), "Received TwistStamped with zero timestamp, setting it to current "
51+
"time, this message will only be shown once");
52+
msg->header.stamp = get_node()->now();
53+
}
54+
55+
if (msg->header.frame_id != controller_params_.tf_prefix + "base") {
56+
// TODO(urfeex): Support transforming the twist?
57+
RCLCPP_WARN(get_node()->get_logger(),
58+
"Received TwistStamped with frame_id '%s'. Currently, it is only supported to command twists in "
59+
"'%s'. The command will be ignored.",
60+
msg->header.frame_id.c_str(), (controller_params_.tf_prefix + "base").c_str());
61+
return;
62+
}
63+
64+
// TODO(urfeex): Check timestamp. Do not accept old commands.
65+
66+
received_twist_msg_.set(*msg);
67+
});
68+
69+
return controller_interface::CallbackReturn::SUCCESS;
70+
}
71+
72+
controller_interface::InterfaceConfiguration TwistController::command_interface_configuration() const
73+
{
74+
// No command interfaces currently
75+
controller_interface::InterfaceConfiguration config;
76+
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
77+
78+
const std::string tf_prefix = controller_params_.tf_prefix;
79+
config.names.push_back(tf_prefix + "twist/linear_velocity_x");
80+
config.names.push_back(tf_prefix + "twist/linear_velocity_y");
81+
config.names.push_back(tf_prefix + "twist/linear_velocity_z");
82+
config.names.push_back(tf_prefix + "twist/angular_velocity_x");
83+
config.names.push_back(tf_prefix + "twist/angular_velocity_y");
84+
config.names.push_back(tf_prefix + "twist/angular_velocity_z");
85+
86+
return config;
87+
}
88+
89+
controller_interface::InterfaceConfiguration TwistController::state_interface_configuration() const
90+
{
91+
// TODO(urfeex): Add state interfaces for tcp speed
92+
93+
controller_interface::InterfaceConfiguration config;
94+
config.type = controller_interface::interface_configuration_type::NONE;
95+
96+
return config;
97+
}
98+
99+
controller_interface::CallbackReturn TwistController::on_activate(const rclcpp_lifecycle::State& state)
100+
{
101+
reset();
102+
subscriber_is_active_ = true;
103+
104+
return ControllerInterface::on_activate(state);
105+
}
106+
107+
controller_interface::CallbackReturn TwistController::on_deactivate(const rclcpp_lifecycle::State&)
108+
{
109+
subscriber_is_active_ = false;
110+
reset();
111+
return CallbackReturn::SUCCESS;
112+
}
113+
114+
controller_interface::return_type TwistController::update(const rclcpp::Time& /*time*/,
115+
const rclcpp::Duration& /*period*/)
116+
{
117+
// In case we can't read from the subscriber's buffer, we will not update the command_msg_.
118+
auto current_cmd = received_twist_msg_.try_get();
119+
if (current_cmd.has_value()) {
120+
command_msg_ = current_cmd.value();
121+
}
122+
123+
// TODO(urfeex): Check if the command_msg_ is valid, e.g., if the timestamp is not too old.
124+
125+
bool success = true;
126+
success &= command_interfaces_[0].set_value(command_msg_.twist.linear.x);
127+
success &= command_interfaces_[1].set_value(command_msg_.twist.linear.y);
128+
success &= command_interfaces_[2].set_value(command_msg_.twist.linear.z);
129+
success &= command_interfaces_[3].set_value(command_msg_.twist.angular.x);
130+
success &= command_interfaces_[4].set_value(command_msg_.twist.angular.y);
131+
success &= command_interfaces_[5].set_value(command_msg_.twist.angular.z);
132+
133+
if (!success) {
134+
RCLCPP_ERROR(get_node()->get_logger(), "Failed to write to command interfaces.");
135+
return controller_interface::return_type::ERROR;
136+
}
137+
138+
return controller_interface::return_type::OK;
139+
}
140+
141+
void TwistController::reset()
142+
{
143+
command_msg_.twist.linear.x = 0.0;
144+
command_msg_.twist.linear.y = 0.0;
145+
command_msg_.twist.linear.z = 0.0;
146+
command_msg_.twist.angular.x = 0.0;
147+
command_msg_.twist.angular.y = 0.0;
148+
command_msg_.twist.angular.z = 0.0;
149+
received_twist_msg_.set(command_msg_);
150+
}
151+
} // namespace ur_controllers
152+
153+
#include "pluginlib/class_list_macros.hpp"
154+
155+
PLUGINLIB_EXPORT_CLASS(ur_controllers::TwistController, controller_interface::ControllerInterface)

‎ur_controllers/src/twist_controller_parameters.yaml‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
---
2+
twist_controller:
3+
tf_prefix: {
4+
type: string,
5+
default_value: "",
6+
description: "Urdf prefix of the corresponding arm"
7+
}

‎ur_robot_driver/config/ur_controllers.yaml‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,9 @@ controller_manager:
4545
tool_contact_controller:
4646
type: ur_controllers/ToolContactController
4747

48+
twist_controller:
49+
type: ur_controllers/TwistController
50+
4851
# The way this is currently implemented upstream doesn't really work for us. When using
4952
# position control, the robot will have a tracking error. However, limits will be enforced
5053
# from the currently reported position, effectively limiting the possible step size using the
@@ -69,6 +72,10 @@ ur_configuration_controller:
6972
ros__parameters:
7073
tf_prefix: "$(var tf_prefix)"
7174

75+
twist_controller:
76+
ros__parameters:
77+
tf_prefix: "$(var tf_prefix)"
78+
7279
force_torque_sensor_broadcaster:
7380
ros__parameters:
7481
sensor_name: $(var tf_prefix)tcp_fts_sensor

‎ur_robot_driver/launch/ur_control.launch.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -206,6 +206,7 @@ def controller_spawner(controllers, active=True):
206206
"freedrive_mode_controller",
207207
"tool_contact_controller",
208208
"motion_primitive_forward_controller",
209+
"twist_controller",
209210
]
210211
if activate_joint_controller.perform(context) == "true":
211212
controllers_active.append(initial_joint_controller.perform(context))

0 commit comments

Comments
 (0)