Skip to content

Commit d40e9c2

Browse files
livanov93livanov93
authored
Merge pull request #12 from PickNikRobotics/livanov/add_force_torque_controller
Integrated force torque sensor controller
2 parents b7dde30 + 7d363ce commit d40e9c2

File tree

10 files changed

+349
-34
lines changed

10 files changed

+349
-34
lines changed

‎.clang-tidy‎

Lines changed: 0 additions & 10 deletions
This file was deleted.

‎ur_controllers/CMakeLists.txt‎

Lines changed: 33 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,35 +1,54 @@
11
cmake_minimum_required(VERSION 3.5)
22
project(ur_controllers)
33

4-
add_compile_options(-std=c++11)
4+
# Default to C++14
5+
if(NOT CMAKE_CXX_STANDARD)
6+
set(CMAKE_CXX_STANDARD 14)
7+
endif()
8+
9+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
10+
add_compile_options(-Wall -Wextra)
11+
endif()
512

613
find_package(ament_cmake REQUIRED)
714
find_package(controller_interface REQUIRED)
8-
find_package(hardware_interface REQUIRED)
915
find_package(joint_trajectory_controller REQUIRED)
1016
find_package(pluginlib REQUIRED)
11-
find_package(realtime_tools REQUIRED)
1217
find_package(std_msgs REQUIRED)
18+
find_package(geometry_msgs REQUIRED)
19+
find_package(rclcpp_lifecycle REQUIRED)
20+
find_package(rcutils REQUIRED)
1321

1422
set(THIS_PACKAGE_INCLUDE_DEPENDS
1523
controller_interface
16-
hardware_interface
1724
joint_trajectory_controller
1825
pluginlib
19-
realtime_tools
2026
std_msgs
27+
geometry_msgs
28+
rclcpp_lifecycle
29+
rcutils
2130
)
2231

2332
include_directories(include)
2433

2534
add_library(${PROJECT_NAME} SHARED
2635
src/scaled_joint_trajectory_controller.cpp
2736
src/speed_scaling_state_controller.cpp
37+
src/force_torque_sensor_controller.cpp
38+
src/force_torque_sensor_controller.cpp)
39+
40+
target_include_directories(${PROJECT_NAME} PRIVATE
41+
include
2842
)
43+
2944
ament_target_dependencies(${PROJECT_NAME}
3045
${THIS_PACKAGE_INCLUDE_DEPENDS}
3146
)
3247

48+
# prevent pluginlib from using boost
49+
target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
50+
pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml)
51+
3352
install(TARGETS ${PROJECT_NAME}
3453
ARCHIVE DESTINATION lib
3554
LIBRARY DESTINATION lib
@@ -46,4 +65,13 @@ install(FILES controller_plugins.xml
4665

4766
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
4867

68+
ament_export_include_directories(
69+
include
70+
)
71+
72+
ament_export_libraries(
73+
${PROJECT_NAME}
74+
)
75+
76+
4977
ament_package()

‎ur_controllers/controller_plugins.xml‎

Lines changed: 5 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,7 @@
1-
<library path="lib/libur_controllers">
2-
<class name="ur_controllers/SpeedScalingStateController" type="ur_controllers::SpeedScalingStateController" base_class_type="controller_interface::ControllerBase">
3-
<description>
4-
This controller publishes the readings of all available speed scaling factors.
5-
</description>
6-
</class>
7-
<class name="position_controllers/ScaledJointTrajectoryController" type="position_controllers::ScaledJointTrajectoryController" base_class_type="controller_interface::ControllerBase">
8-
<description>
9-
Scaled velocity-based joint trajectory controller
10-
</description>
11-
</class>
12-
<class name="velocity_controllers/ScaledJointTrajectoryController" type="velocity_controllers::ScaledJointTrajectoryController" base_class_type="controller_interface::ControllerBase">
13-
<description>
14-
Scaled velocity-based joint trajectory controller
15-
</description>
1+
<library path="ur_controllers">
2+
<class name="ur_controllers/ForceTorqueStateController" type="ur_controllers::ForceTorqueStateController" base_class_type="controller_interface::ControllerInterface">
3+
<description>
4+
The force torque state controller publishes the current force and torques
5+
</description>
166
</class>
177
</library>

‎ur_controllers/include/ur_controllers/force_torque_sensor_controller.h‎

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
// Copyright (c) 2021 PickNik LLC
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef UR_CONTROLLERS_FORCE_TORQUE_SENSOR_CONTROLLER_H
16+
#define UR_CONTROLLERS_FORCE_TORQUE_SENSOR_CONTROLLER_H
17+
18+
#include <memory>
19+
#include <string>
20+
#include <unordered_map>
21+
#include <vector>
22+
23+
#include <controller_interface/controller_interface.hpp>
24+
25+
#include <geometry_msgs/msg/wrench_stamped.hpp>
26+
27+
namespace ur_controllers
28+
{
29+
using FbkType = geometry_msgs::msg::WrenchStamped;
30+
31+
struct FTStateControllerParams
32+
{
33+
FTStateControllerParams()
34+
{
35+
sensor_name_ = "";
36+
state_interfaces_names_ = {};
37+
frame_id = "";
38+
topic_name = "";
39+
}
40+
41+
std::string sensor_name_;
42+
std::vector<std::string> state_interfaces_names_;
43+
std::string frame_id;
44+
std::string topic_name;
45+
};
46+
47+
/**
48+
* \brief Force torque state controller.
49+
*
50+
* This class forwards the command signal down to a set of joints
51+
* on the specified interface.
52+
*
53+
* \param topic_name Name of the topic to publish WrenchStamped msg
54+
* \param frame_id Frame of the ft sensor
55+
* \param sensor_name Names of the sensor which data should be published .
56+
* \param state_interface_names Names of the state interfaces in the sensor.
57+
*
58+
* Publishes to:
59+
* - \b topic_name (geometry_msg::msg::WrenchStamped) : Sensor data.
60+
*/
61+
class ForceTorqueStateController : public controller_interface::ControllerInterface
62+
{
63+
public:
64+
ForceTorqueStateController();
65+
66+
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
67+
68+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
69+
70+
controller_interface::return_type update() override;
71+
72+
CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
73+
74+
CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
75+
76+
CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
77+
78+
controller_interface::return_type init(const std::string& controller_name) override;
79+
80+
protected:
81+
// param storage
82+
FTStateControllerParams fts_params_;
83+
std::shared_ptr<rclcpp::Publisher<FbkType>> wrench_state_publisher_;
84+
geometry_msgs::msg::WrenchStamped wrench_state_msg_;
85+
};
86+
} // namespace ur_controllers
87+
88+
#endif // UR_CONTROLLERS_FORCE_TORQUE_SENSOR_CONTROLLER_H

‎ur_controllers/package.xml‎

Lines changed: 11 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -11,12 +11,20 @@
1111

1212
<buildtool_depend>ament_cmake</buildtool_depend>
1313

14+
<build_depend>pluginlib</build_depend>
15+
<build_depend>rcutils</build_depend>
16+
17+
<exec_depend>pluginlib</exec_depend>
18+
<exec_depend>rcutils</exec_depend>
19+
1420
<depend>controller_interface</depend>
15-
<depend>hardware_interface</depend>
21+
<depend>rclcpp_lifecycle</depend>
22+
<depend>geometry_msgs</depend>
1623
<depend>joint_trajectory_controller</depend>
17-
<depend>pluginlib</depend>
18-
<depend>realtime_tools</depend>
1924
<depend>std_msgs</depend>
25+
<depend>std_srvs</depend>
26+
<depend>ur_client_library</depend>
27+
<depend>ur_dashboard_msgs</depend>
2028

2129
<export>
2230
<build_type>ament_cmake</build_type>

‎ur_controllers/src/force_torque_sensor_controller.cpp‎

Lines changed: 178 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,178 @@
1+
// Copyright (c) 2021 PickNik LLC
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "ur_controllers/force_torque_sensor_controller.h"
16+
17+
namespace ur_controllers
18+
{
19+
ForceTorqueStateController::ForceTorqueStateController()
20+
: controller_interface::ControllerInterface(), wrench_state_publisher_(nullptr)
21+
{
22+
}
23+
24+
controller_interface::return_type ForceTorqueStateController::init(const std::string& controller_name)
25+
{
26+
auto ret = ControllerInterface::init(controller_name);
27+
if (ret != controller_interface::return_type::SUCCESS)
28+
{
29+
return ret;
30+
}
31+
32+
try
33+
{
34+
auto node = get_node();
35+
node->declare_parameter<std::vector<std::string>>("state_interface_names", {});
36+
node->declare_parameter<std::string>("sensor_name", "");
37+
node->declare_parameter<std::string>("topic_name", "");
38+
node->declare_parameter<std::string>("frame_id", "");
39+
}
40+
catch (const std::exception& e)
41+
{
42+
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
43+
return controller_interface::return_type::ERROR;
44+
}
45+
46+
return controller_interface::return_type::SUCCESS;
47+
}
48+
49+
controller_interface::InterfaceConfiguration ForceTorqueStateController::command_interface_configuration() const
50+
{
51+
controller_interface::InterfaceConfiguration config;
52+
config.type = controller_interface::interface_configuration_type::NONE;
53+
return config;
54+
}
55+
56+
controller_interface::InterfaceConfiguration ForceTorqueStateController::state_interface_configuration() const
57+
{
58+
controller_interface::InterfaceConfiguration config;
59+
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
60+
for (const auto& si : fts_params_.state_interfaces_names_)
61+
{
62+
config.names.push_back(fts_params_.sensor_name_ + "/" + si);
63+
}
64+
65+
return config;
66+
}
67+
68+
controller_interface::return_type ur_controllers::ForceTorqueStateController::update()
69+
{
70+
geometry_msgs::msg::Vector3 f_vec;
71+
geometry_msgs::msg::Vector3 t_vec;
72+
73+
if (state_interfaces_.size() != fts_params_.state_interfaces_names_.size())
74+
return controller_interface::return_type::ERROR;
75+
76+
for (auto index = 0ul; index < state_interfaces_.size(); ++index)
77+
{
78+
switch (index)
79+
{
80+
case 0:
81+
f_vec.set__x(state_interfaces_[index].get_value());
82+
break;
83+
case 1:
84+
f_vec.set__y(state_interfaces_[index].get_value());
85+
break;
86+
case 2:
87+
f_vec.set__z(state_interfaces_[index].get_value());
88+
break;
89+
case 3:
90+
t_vec.set__x(state_interfaces_[index].get_value());
91+
break;
92+
case 4:
93+
t_vec.set__y(state_interfaces_[index].get_value());
94+
break;
95+
case 5:
96+
t_vec.set__z(state_interfaces_[index].get_value());
97+
break;
98+
default:
99+
break;
100+
}
101+
}
102+
103+
wrench_state_msg_.header.stamp = get_node()->get_clock()->now();
104+
wrench_state_msg_.header.frame_id = fts_params_.frame_id;
105+
106+
// update wrench state message
107+
wrench_state_msg_.wrench.set__force(f_vec);
108+
wrench_state_msg_.wrench.set__torque(t_vec);
109+
110+
// publish
111+
wrench_state_publisher_->publish(wrench_state_msg_);
112+
113+
return controller_interface::return_type::SUCCESS;
114+
}
115+
116+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
117+
ForceTorqueStateController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
118+
{
119+
fts_params_.state_interfaces_names_ = node_->get_parameter("state_interface_names").as_string_array();
120+
121+
if (fts_params_.state_interfaces_names_.empty())
122+
{
123+
RCLCPP_ERROR(get_node()->get_logger(), "'state_interface_names' parameter was empty");
124+
return CallbackReturn::ERROR;
125+
}
126+
127+
fts_params_.sensor_name_ = node_->get_parameter("sensor_name").as_string();
128+
if (fts_params_.sensor_name_.empty())
129+
{
130+
RCLCPP_ERROR(get_node()->get_logger(), "'sensor_name' parameter was empty");
131+
return CallbackReturn::ERROR;
132+
}
133+
134+
fts_params_.topic_name = node_->get_parameter("topic_name").as_string();
135+
if (fts_params_.topic_name.empty())
136+
{
137+
RCLCPP_ERROR(get_node()->get_logger(), "'topic_name' parameter was empty");
138+
return CallbackReturn::ERROR;
139+
}
140+
141+
fts_params_.frame_id = node_->get_parameter("frame_id").as_string();
142+
if (fts_params_.frame_id.empty())
143+
{
144+
RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter was empty");
145+
return CallbackReturn::ERROR;
146+
}
147+
148+
try
149+
{
150+
// register ft sensor data publisher
151+
wrench_state_publisher_ = get_node()->create_publisher<geometry_msgs::msg::WrenchStamped>(
152+
fts_params_.topic_name, rclcpp::SystemDefaultsQoS());
153+
}
154+
catch (...)
155+
{
156+
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
157+
}
158+
159+
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
160+
}
161+
162+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
163+
ForceTorqueStateController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
164+
{
165+
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
166+
}
167+
168+
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
169+
ForceTorqueStateController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
170+
{
171+
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
172+
}
173+
174+
} // namespace ur_controllers
175+
176+
#include "pluginlib/class_list_macros.hpp"
177+
178+
PLUGINLIB_EXPORT_CLASS(ur_controllers::ForceTorqueStateController, controller_interface::ControllerInterface)

0 commit comments

Comments
 (0)