From adacb219c7c2867899b595586e0640389d220cac Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Wed, 12 Jun 2019 11:56:24 +0200 Subject: [PATCH] corrected naming scheme of fts-transformation --- .../include/ur_rtde_driver/ros/hardware_interface.h | 2 +- ur_rtde_driver/src/ros/hardware_interface.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h index 3c13aa6..4422274 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h +++ b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h @@ -73,7 +73,7 @@ protected: /*! * \brief Transforms force-torque measurements reported from the robot from base to tool frame */ - void transform_force_torque(); + void transformForceTorque(); std::unique_ptr ur_driver_; diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index db2c53b..d368fc2 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -226,7 +226,7 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period } // Transform fts measurements to tool frame - transform_force_torque(); + transformForceTorque(); // pausing state follows runtime state when pausing if (runtime_state_ == static_cast(rtde_interface::RUNTIME_STATE::PAUSED)) @@ -312,7 +312,7 @@ uint32_t HardwareInterface ::getControlFrequency() const throw std::runtime_error("ur_driver is not yet initialized"); } -void HardwareInterface ::transform_force_torque() +void HardwareInterface ::transformForceTorque() { double tcp_angle = std::sqrt(std::pow(tcp_pose_[3], 2) + std::pow(tcp_pose_[4], 2) + std::pow(tcp_pose_[5], 2));