diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt
index a2b72e599..cd9ad55c5 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(hardware_interface 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_msgs 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
+ hardware_interface
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_msgs
ur_dashboard_msgs
ur_msgs
generate_parameter_library
@@ -54,7 +60,13 @@ generate_parameter_library(
src/scaled_joint_trajectory_controller_parameters.yaml
)
+generate_parameter_library(
+ pose_broadcaster_parameters
+ src/pose_broadcaster_parameters.yaml
+)
+
add_library(${PROJECT_NAME} SHARED
+ src/pose_broadcaster.cpp
src/scaled_joint_trajectory_controller.cpp
src/speed_scaling_state_broadcaster.cpp
src/gpio_controller.cpp)
@@ -64,8 +76,9 @@ target_include_directories(${PROJECT_NAME} PRIVATE
)
target_link_libraries(${PROJECT_NAME}
gpio_controller_parameters
- speed_scaling_state_broadcaster_parameters
+ pose_broadcaster_parameters
scaled_joint_trajectory_controller_parameters
+ speed_scaling_state_broadcaster_parameters
)
ament_target_dependencies(${PROJECT_NAME}
${THIS_PACKAGE_INCLUDE_DEPENDS}
diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml
index f0058ab55..0b862f23b 100644
--- a/ur_controllers/controller_plugins.xml
+++ b/ur_controllers/controller_plugins.xml
@@ -14,4 +14,9 @@
This controller publishes the Tool IO.
+
+
+ This controller publishes a tf2 message.
+
+
diff --git a/ur_controllers/include/ur_controllers/pose_broadcaster.hpp b/ur_controllers/include/ur_controllers/pose_broadcaster.hpp
new file mode 100644
index 000000000..34f4ab577
--- /dev/null
+++ b/ur_controllers/include/ur_controllers/pose_broadcaster.hpp
@@ -0,0 +1,84 @@
+// Copyright 2023, FZI Forschungszentrum Informatik
+//
+// 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
+ * \date 2023-09-11
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_CONTROLLERS__POSE_BROADCASTER_HPP
+#define UR_CONTROLLERS__POSE_BROADCASTER_HPP
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "pose_broadcaster_parameters.hpp"
+#include "tf2_msgs/msg/tf_message.hpp"
+
+#include
+#include
+#include
+#include
+
+namespace ur_controllers
+{
+class PoseBroadcaster : public controller_interface::ControllerInterface
+{
+public:
+ PoseBroadcaster();
+ virtual ~PoseBroadcaster() = default;
+ 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;
+
+ controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+
+ controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+
+ controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+ controller_interface::CallbackReturn on_init() override;
+
+private:
+ std::unique_ptr pose_component_;
+ std::shared_ptr param_listener_;
+ pose_broadcaster::Params params_;
+
+ using StatePublisher = realtime_tools::RealtimePublisher;
+ rclcpp::Publisher::SharedPtr sensor_state_publisher_;
+ std::unique_ptr realtime_publisher_;
+
+ geometry_msgs::msg::Pose pose_;
+};
+} // namespace ur_controllers
+
+#endif // ifndef UR_CONTROLLERS__POSE_BROADCASTER_HPP
diff --git a/ur_controllers/include/ur_controllers/sc_pose.hpp b/ur_controllers/include/ur_controllers/sc_pose.hpp
new file mode 100644
index 000000000..03d2897a6
--- /dev/null
+++ b/ur_controllers/include/ur_controllers/sc_pose.hpp
@@ -0,0 +1,119 @@
+// Copyright 2023, FZI Forschungszentrum Informatik
+//
+// 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
+ * \date 2023-09-11
+ *
+ */
+//----------------------------------------------------------------------
+
+#pragma once
+
+#include
+
+#include
+#include
+#include
+
+#include
+#include
+
+#include
+#include
+
+namespace ur_controllers
+{
+class PoseComponent : public semantic_components::SemanticComponentInterface
+{
+public:
+ explicit PoseComponent(const std::string& name) : SemanticComponentInterface(name, 6)
+ {
+ interface_names_.emplace_back(name_ + "/" + "position.x");
+ interface_names_.emplace_back(name_ + "/" + "position.y");
+ interface_names_.emplace_back(name_ + "/" + "position.z");
+ interface_names_.emplace_back(name_ + "/" + "orientation.x");
+ interface_names_.emplace_back(name_ + "/" + "orientation.y");
+ interface_names_.emplace_back(name_ + "/" + "orientation.z");
+ interface_names_.emplace_back(name_ + "/" + "orientation.w");
+
+ std::fill(position_.begin(), position_.end(), std::numeric_limits::quiet_NaN());
+ std::fill(orientation_.begin(), orientation_.end(), std::numeric_limits::quiet_NaN());
+ }
+
+ virtual ~PoseComponent() = default;
+
+ /**
+ * Return Pose message with full pose, parent and child frame
+ *
+ * \return Pose from values;
+ */
+ bool get_values_as_message(geometry_msgs::msg::Pose& message)
+ {
+ get_position();
+ get_orientation();
+
+ // update the message values
+ message.position.x = position_[0];
+ message.position.y = position_[1];
+ message.position.z = position_[2];
+
+ message.orientation.x = orientation_[0];
+ message.orientation.y = orientation_[1];
+ message.orientation.z = orientation_[2];
+ message.orientation.w = orientation_[3];
+
+ return true;
+ }
+
+protected:
+ std::array get_position()
+ {
+ size_t interface_counter = 0;
+ for (size_t i = 0; i < 3; ++i) {
+ position_[i] = state_interfaces_[interface_counter].get().get_value();
+ ++interface_counter;
+ }
+ return position_;
+ }
+ std::array get_orientation()
+ {
+ size_t interface_counter = 3;
+ for (size_t i = 0; i < 4; ++i) {
+ orientation_[i] = state_interfaces_[interface_counter].get().get_value();
+ ++interface_counter;
+ }
+ return orientation_;
+ }
+ std::array position_;
+ std::array orientation_;
+};
+
+} // namespace ur_controllers
diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml
index e8a492e3f..f7010ccdf 100644
--- a/ur_controllers/package.xml
+++ b/ur_controllers/package.xml
@@ -22,6 +22,7 @@
angles
controller_interface
+ hardware_interface
joint_trajectory_controller
lifecycle_msgs
pluginlib
@@ -30,6 +31,8 @@
realtime_tools
std_msgs
std_srvs
+ tf2_geometry_msgs
+ tf2_msgs
ur_dashboard_msgs
ur_msgs
diff --git a/ur_controllers/src/pose_broadcaster.cpp b/ur_controllers/src/pose_broadcaster.cpp
new file mode 100644
index 000000000..e99a8492c
--- /dev/null
+++ b/ur_controllers/src/pose_broadcaster.cpp
@@ -0,0 +1,134 @@
+// Copyright 2023, FZI Forschungszentrum Informatik
+//
+// 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
+ * \date 2023-09-11
+ *
+ */
+//----------------------------------------------------------------------
+//
+#include "ur_controllers/pose_broadcaster.hpp"
+#include
+#include "tf2_msgs/msg/tf_message.hpp"
+
+namespace ur_controllers
+{
+
+PoseBroadcaster::PoseBroadcaster() : controller_interface::ControllerInterface()
+{
+}
+
+controller_interface::InterfaceConfiguration PoseBroadcaster::command_interface_configuration() const
+{
+ controller_interface::InterfaceConfiguration command_interfaces_config;
+ command_interfaces_config.type = controller_interface::interface_configuration_type::NONE;
+ return command_interfaces_config;
+}
+
+controller_interface::InterfaceConfiguration PoseBroadcaster::state_interface_configuration() const
+{
+ controller_interface::InterfaceConfiguration state_interfaces_config;
+ state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+ state_interfaces_config.names = pose_component_->get_state_interface_names();
+ return state_interfaces_config;
+}
+
+controller_interface::return_type PoseBroadcaster::update(const rclcpp::Time& time, const rclcpp::Duration& period)
+{
+ if (realtime_publisher_ && realtime_publisher_->trylock()) {
+ pose_component_->get_values_as_message(pose_);
+ realtime_publisher_->msg_.transforms[0].header.stamp = time;
+ realtime_publisher_->msg_.transforms[0].transform.translation.x = pose_.position.x;
+ realtime_publisher_->msg_.transforms[0].transform.translation.y = pose_.position.y;
+ realtime_publisher_->msg_.transforms[0].transform.translation.z = pose_.position.z;
+ realtime_publisher_->msg_.transforms[0].transform.rotation.w = pose_.orientation.w;
+ realtime_publisher_->msg_.transforms[0].transform.rotation.x = pose_.orientation.x;
+ realtime_publisher_->msg_.transforms[0].transform.rotation.y = pose_.orientation.y;
+ realtime_publisher_->msg_.transforms[0].transform.rotation.z = pose_.orientation.z;
+ realtime_publisher_->unlockAndPublish();
+ }
+
+ return controller_interface::return_type::OK;
+}
+
+controller_interface::CallbackReturn PoseBroadcaster::on_configure(const rclcpp_lifecycle::State& previous_state)
+{
+ try {
+ param_listener_ = std::make_shared(get_node());
+ params_ = param_listener_->get_params();
+ } catch (const std::exception& e) {
+ fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what());
+ return controller_interface::CallbackReturn::ERROR;
+ }
+
+ pose_component_ = std::make_unique(ur_controllers::PoseComponent(params_.sensor_name));
+
+ try {
+ // register sensor data publisher
+ sensor_state_publisher_ =
+ get_node()->create_publisher("/tf", rclcpp::SystemDefaultsQoS());
+ realtime_publisher_ = std::make_unique(sensor_state_publisher_);
+ } catch (const std::exception& e) {
+ RCLCPP_ERROR(get_node()->get_logger(), "Could not create publisher: %s \n", e.what());
+ return controller_interface::CallbackReturn::ERROR;
+ }
+
+ realtime_publisher_->lock();
+ realtime_publisher_->msg_.transforms.resize(1);
+ realtime_publisher_->msg_.transforms[0].header.frame_id = params_.parent_frame_id;
+ realtime_publisher_->msg_.transforms[0].child_frame_id = params_.child_frame_id;
+ realtime_publisher_->unlock();
+
+ RCLCPP_DEBUG(get_node()->get_logger(), "configure successful");
+ return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn PoseBroadcaster::on_activate(const rclcpp_lifecycle::State& previous_state)
+{
+ pose_component_->assign_loaned_state_interfaces(state_interfaces_);
+ return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn PoseBroadcaster::on_deactivate(const rclcpp_lifecycle::State& previous_state)
+{
+ pose_component_->release_interfaces();
+ return controller_interface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn PoseBroadcaster::on_init()
+{
+ return controller_interface::CallbackReturn::SUCCESS;
+}
+
+} // namespace ur_controllers
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(ur_controllers::PoseBroadcaster, controller_interface::ControllerInterface)
diff --git a/ur_controllers/src/pose_broadcaster_parameters.yaml b/ur_controllers/src/pose_broadcaster_parameters.yaml
new file mode 100644
index 000000000..cc5a4d596
--- /dev/null
+++ b/ur_controllers/src/pose_broadcaster_parameters.yaml
@@ -0,0 +1,16 @@
+pose_broadcaster:
+ sensor_name: {
+ type: string,
+ default_value: "tcp_pose",
+ description: "State interface used to encode the pose."
+ }
+ parent_frame_id: {
+ type: string,
+ default_value: "base",
+ description: "Parent frame id of the published transform."
+ }
+ child_frame_id: {
+ type: string,
+ default_value: "tool0_controller",
+ description: "Child frame id of the published transform."
+ }
diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml
index a512dc1ca..b791183c2 100644
--- a/ur_robot_driver/config/ur_controllers.yaml
+++ b/ur_robot_driver/config/ur_controllers.yaml
@@ -12,6 +12,9 @@ controller_manager:
force_torque_sensor_broadcaster:
type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
+ tcp_pose_broadcaster:
+ type: ur_controllers/PoseBroadcaster
+
joint_trajectory_controller:
type: joint_trajectory_controller/JointTrajectoryController
@@ -47,6 +50,12 @@ force_torque_sensor_broadcaster:
frame_id: $(var tf_prefix)tool0
topic_name: ft_data
+tcp_pose_broadcaster:
+ ros__parameters:
+ sensor_name: $(var tf_prefix)flange_pose_sensor
+ child_frame_id: $(var tf_prefix)flange_controller
+ parent_frame_id: $(var tf_prefix)base
+
joint_trajectory_controller:
ros__parameters:
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 63f762f09..0065da985 100644
--- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp
+++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp
@@ -40,6 +40,8 @@
#define UR_ROBOT_DRIVER__HARDWARE_INTERFACE_HPP_
// System
+#include
+#include
#include
#include
#include
@@ -127,6 +129,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
template
void readBitsetData(const std::unique_ptr& data_pkg, const std::string& var_name,
std::bitset& data);
+ tf2::Quaternion quaternionFromURPose(const urcl::vector6d_t& rotation_vec);
void initAsyncIO();
void checkAsyncIO();
@@ -142,6 +145,10 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
urcl::vector6d_t urcl_joint_efforts_;
urcl::vector6d_t urcl_ft_sensor_measurements_;
urcl::vector6d_t urcl_tcp_pose_;
+ urcl::vector6d_t urcl_tcp_target_pose_;
+ urcl::vector6d_t urcl_tcp_offset_;
+ urcl::vector3d_t urcl_flange_position_;
+ std::array urcl_flange_orientation_;
bool packet_read_;
diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py
index 3ac85dbbf..234ae4a49 100644
--- a/ur_robot_driver/launch/ur_control.launch.py
+++ b/ur_robot_driver/launch/ur_control.launch.py
@@ -285,6 +285,7 @@ def launch_setup(context, *args, **kwargs):
"consistent_controllers": [
"io_and_status_controller",
"force_torque_sensor_broadcaster",
+ "tcp_pose_broadcaster",
"joint_state_broadcaster",
"speed_scaling_state_broadcaster",
]
@@ -329,6 +330,7 @@ def controller_spawner(name, active=True):
"io_and_status_controller",
"speed_scaling_state_broadcaster",
"force_torque_sensor_broadcaster",
+ "tcp_pose_broadcaster",
]
controller_spawner_inactive_names = ["forward_position_controller"]
diff --git a/ur_robot_driver/resources/rtde_output_recipe.txt b/ur_robot_driver/resources/rtde_output_recipe.txt
index 8d10be849..c06ebfeb5 100644
--- a/ur_robot_driver/resources/rtde_output_recipe.txt
+++ b/ur_robot_driver/resources/rtde_output_recipe.txt
@@ -6,6 +6,8 @@ target_speed_fraction
runtime_state
actual_TCP_force
actual_TCP_pose
+target_TCP_pose
+tcp_offset
actual_digital_input_bits
actual_digital_output_bits
standard_analog_input0
diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp
index 2434e5248..835cd89c0 100644
--- a/ur_robot_driver/src/hardware_interface.cpp
+++ b/ur_robot_driver/src/hardware_interface.cpp
@@ -36,17 +36,29 @@
*
*/
//----------------------------------------------------------------------
+#include
+#include
+
+#include
+#include
+
#include
+#include
#include
#include
#include
#include
-#include "ur_client_library/exceptions.h"
-#include "ur_client_library/ur/tool_communication.h"
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+#include
-#include "rclcpp/rclcpp.hpp"
-#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "ur_robot_driver/hardware_interface.hpp"
#include "ur_robot_driver/urcl_log_handler.hpp"
@@ -77,6 +89,10 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys
urcl_joint_efforts_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
urcl_ft_sensor_measurements_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
urcl_tcp_pose_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
+ urcl_tcp_target_pose_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
+ urcl_tcp_offset_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
+ urcl_flange_position_ = { { 0.0, 0.0, 0.0 } };
+ urcl_flange_orientation_ = { { 0.0, 0.0, 0.0, 0.0 } };
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 } };
@@ -169,9 +185,20 @@ std::vector URPositionHardwareInterface::exp
&speed_scaling_combined_));
for (auto& sensor : info_.sensors) {
- for (uint j = 0; j < sensor.state_interfaces.size(); ++j) {
- state_interfaces.emplace_back(hardware_interface::StateInterface(sensor.name, sensor.state_interfaces[j].name,
- &urcl_ft_sensor_measurements_[j]));
+ if (sensor.name == tf_prefix + "tcp_fts_sensor") {
+ for (uint j = 0; j < sensor.state_interfaces.size(); ++j) {
+ state_interfaces.emplace_back(hardware_interface::StateInterface(sensor.name, sensor.state_interfaces[j].name,
+ &urcl_ft_sensor_measurements_[j]));
+ }
+ } else if (sensor.name == tf_prefix + "flange_pose_sensor") {
+ for (uint j = 0; j < 3; ++j) {
+ state_interfaces.emplace_back(hardware_interface::StateInterface(sensor.name, sensor.state_interfaces[j].name,
+ &urcl_flange_position_[j]));
+ }
+ for (uint j = 0; j < 4; ++j) {
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ sensor.name, sensor.state_interfaces[j + 3].name, &urcl_flange_orientation_[j]));
+ }
}
}
@@ -421,6 +448,8 @@ URPositionHardwareInterface::on_activate(const rclcpp_lifecycle::State& previous
// distiguishable in the log
const std::string tf_prefix = info_.hardware_parameters.at("tf_prefix");
RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Initializing driver...");
+ tcp_transform_.header.frame_id = tf_prefix + "base";
+ tcp_transform_.child_frame_id = tf_prefix + "tool0_controller";
registerUrclLogHandler(tf_prefix);
try {
rtde_comm_has_been_started_ = false;
@@ -535,6 +564,8 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::
readData(data_pkg, "runtime_state", runtime_state_);
readData(data_pkg, "actual_TCP_force", urcl_ft_sensor_measurements_);
readData(data_pkg, "actual_TCP_pose", urcl_tcp_pose_);
+ readData(data_pkg, "target_TCP_pose", urcl_tcp_target_pose_);
+ readData(data_pkg, "tcp_offset", urcl_tcp_offset_);
readData(data_pkg, "standard_analog_input0", standard_analog_input_[0]);
readData(data_pkg, "standard_analog_input1", standard_analog_input_[1]);
readData(data_pkg, "standard_analog_output0", standard_analog_output_[0]);
@@ -766,23 +797,37 @@ void URPositionHardwareInterface::transformForceTorque()
tcp_torque_.x(), tcp_torque_.y(), tcp_torque_.z() };
}
-void URPositionHardwareInterface::extractToolPose()
+tf2::Quaternion URPositionHardwareInterface::quaternionFromURPose(const urcl::vector6d_t& pose)
{
- // imported from ROS1 driver hardware_interface.cpp#L911-L928
- double tcp_angle =
- std::sqrt(std::pow(urcl_tcp_pose_[3], 2) + std::pow(urcl_tcp_pose_[4], 2) + std::pow(urcl_tcp_pose_[5], 2));
-
- tf2::Vector3 rotation_vec(urcl_tcp_pose_[3], urcl_tcp_pose_[4], urcl_tcp_pose_[5]);
- tf2::Quaternion rotation;
- if (tcp_angle > 1e-16) {
- rotation.setRotation(rotation_vec.normalized(), tcp_angle);
- } else {
- rotation.setValue(0.0, 0.0, 0.0, 1.0); // default Quaternion is 0,0,0,0 which is invalid
+ double rot_angle = std::sqrt(std::pow(pose[3], 2) + std::pow(pose[4], 2) + std::pow(pose[5], 2));
+ if (rot_angle > 1e-16) {
+ return tf2::Quaternion{ { pose[3] / rot_angle, pose[4] / rot_angle, pose[5] / rot_angle }, rot_angle };
}
- tcp_transform_.transform.translation.x = urcl_tcp_pose_[0];
- tcp_transform_.transform.translation.y = urcl_tcp_pose_[1];
- tcp_transform_.transform.translation.z = urcl_tcp_pose_[2];
+ return tf2::Quaternion(0, 0, 0, 1);
+}
+
+void URPositionHardwareInterface::extractToolPose()
+{
+ tf2::Quaternion rot_tool = quaternionFromURPose(urcl_tcp_pose_);
+ tf2::Quaternion rot_offset = quaternionFromURPose(urcl_tcp_offset_);
+
+ tf2::Vector3 tcp_offset_in_tcp = tf2::quatRotate(
+ rot_offset.inverse(), tf2::Vector3(urcl_tcp_offset_[0], urcl_tcp_offset_[1], urcl_tcp_offset_[2]));
+ tf2::Vector3 tcp_offset_in_base = tf2::quatRotate(rot_tool, tcp_offset_in_tcp);
+
+ urcl_flange_position_[0] = urcl_tcp_pose_[0] - tcp_offset_in_base.x();
+ urcl_flange_position_[1] = urcl_tcp_pose_[1] - tcp_offset_in_base.y();
+ urcl_flange_position_[2] = urcl_tcp_pose_[2] - tcp_offset_in_base.z();
+ tcp_transform_.transform.translation.x = urcl_flange_position_[0];
+ tcp_transform_.transform.translation.y = urcl_flange_position_[1];
+ tcp_transform_.transform.translation.z = urcl_flange_position_[2];
+
+ tf2::Quaternion rotation = rot_tool * rot_offset.inverse();
+ urcl_flange_orientation_[0] = rotation.x();
+ urcl_flange_orientation_[1] = rotation.y();
+ urcl_flange_orientation_[2] = rotation.z();
+ urcl_flange_orientation_[3] = rotation.w();
tcp_transform_.transform.rotation = tf2::toMsg(rotation);
}