Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Transform force-torque into tcp frame. #237

Merged
merged 2 commits into from Nov 10, 2021
Merged

Transform force-torque into tcp frame. #237

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
bf9bbbe
Transform force-torque into tcp frame.
livanov93 Nov 5, 2021
413880b
Point out the original source of the code.
livanov93 Nov 10, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
2 changes: 2 additions & 0 deletions ur_robot_driver/CMakeLists.txt
View file
Open in desktop
Expand Up @@ -16,7 +16,9 @@ find_package(hardware_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(ur_client_library REQUIRED)
find_package(ur_dashboard_msgs REQUIRED)

Expand Down
9 changes: 9 additions & 0 deletions ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp
View file
Open in desktop
Expand Up @@ -45,6 +45,8 @@
// ROS
#include "rclcpp/macros.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

namespace ur_robot_driver
{
Expand Down Expand Up @@ -113,6 +115,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
void initAsyncIO();
void checkAsyncIO();
void updateNonDoubleValues();
void extractToolPose();
void transformForceTorque();

urcl::vector6d_t urcl_position_commands_;
urcl::vector6d_t urcl_position_commands_old_;
Expand Down Expand Up @@ -147,6 +151,11 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
std::bitset<4> robot_status_bits_;
std::bitset<11> safety_status_bits_;

// transform stuff
tf2::Vector3 tcp_force_;
tf2::Vector3 tcp_torque_;
geometry_msgs::msg::TransformStamped tcp_transform_;

// asynchronous commands
std::array<double, 18> standard_dig_out_bits_cmd_;
std::array<double, 2> standard_analog_output_cmd_;
Expand Down
2 changes: 2 additions & 0 deletions ur_robot_driver/package.xml
View file
Open in desktop
Expand Up @@ -27,6 +27,8 @@
<depend>pluginlib</depend>
<depend>rclcpp</depend>
<depend>std_srvs</depend>
<depend>geometry_msgs</depend>
<depend>tf2_geometry_msgs</depend>
<!-- TODO: rosdep cannot find ur_client_library -->
<depend>ur_client_library</depend>
<depend>ur_dashboard_msgs</depend>
Expand Down
41 changes: 41 additions & 0 deletions ur_robot_driver/src/hardware_interface.cpp
View file
Open in desktop
Expand Up @@ -467,6 +467,10 @@ hardware_interface::return_type URPositionHardwareInterface::read()
readBitsetData<uint32_t>(data_pkg, "analog_io_types", analog_io_types_);
readBitsetData<uint32_t>(data_pkg, "tool_analog_input_types", tool_analog_input_types_);

// required transforms
extractToolPose();
transformForceTorque();

// TODO(anyone): logic for sending other stuff to higher level interface

// pausing state follows runtime state when pausing
Expand Down Expand Up @@ -623,6 +627,43 @@ void URPositionHardwareInterface::updateNonDoubleValues()
system_interface_initialized_ = initialized_ ? 1.0 : 0.0;
}

void URPositionHardwareInterface::transformForceTorque()
{
// imported from ROS1 driver - hardware_interface.cpp#L867-L876
tcp_force_.setValue(urcl_ft_sensor_measurements_[0], urcl_ft_sensor_measurements_[1],
urcl_ft_sensor_measurements_[2]);
tcp_torque_.setValue(urcl_ft_sensor_measurements_[3], urcl_ft_sensor_measurements_[4],
urcl_ft_sensor_measurements_[5]);

tf2::Quaternion rotation_quat;
tf2::fromMsg(tcp_transform_.transform.rotation, rotation_quat);
tcp_force_ = tf2::quatRotate(rotation_quat.inverse(), tcp_force_);
tcp_torque_ = tf2::quatRotate(rotation_quat.inverse(), tcp_torque_);

urcl_ft_sensor_measurements_ = { tcp_force_.x(), tcp_force_.y(), tcp_force_.z(),
tcp_torque_.x(), tcp_torque_.y(), tcp_torque_.z() };
}

void URPositionHardwareInterface::extractToolPose()
{
// 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
}
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];

tcp_transform_.transform.rotation = tf2::toMsg(rotation);
}

hardware_interface::return_type URPositionHardwareInterface::prepare_command_mode_switch(
const std::vector<std::string>& start_interfaces, const std::vector<std::string>& stop_interfaces)
{
Expand Down