From ccdfb896124e7ad667b33f7965611c5e59bf4519 Mon Sep 17 00:00:00 2001 From: Hydran00 Date: Wed, 11 Dec 2024 18:25:57 +0100 Subject: [PATCH] solved timing bugs --- .../src/raptor_api_wrapper.cpp | 2 + src/vf_control/config/parameters.yaml | 4 +- src/vf_control/include/circular_buffer.hpp | 105 ++++++++++++++++++ .../include/haptic_control_base.hpp | 11 +- src/vf_control/include/system_interface.hpp | 2 + src/vf_control/launch/vf.launch.py | 9 +- src/vf_control/src/haptic_control_base.cpp | 63 +++++++---- 7 files changed, 170 insertions(+), 26 deletions(-) create mode 100644 src/vf_control/include/circular_buffer.hpp diff --git a/src/haption_raptor_api/src/raptor_api_wrapper.cpp b/src/haption_raptor_api/src/raptor_api_wrapper.cpp index 8482fda..3137e63 100644 --- a/src/haption_raptor_api/src/raptor_api_wrapper.cpp +++ b/src/haption_raptor_api/src/raptor_api_wrapper.cpp @@ -288,6 +288,8 @@ class RaptorAPIWrapper : public rclcpp::Node { break; case HAPTION::CalibrationStatus::C_IDLE: break; + default: + break; } } // Update status diff --git a/src/vf_control/config/parameters.yaml b/src/vf_control/config/parameters.yaml index 48b1b9f..b6d8d34 100755 --- a/src/vf_control/config/parameters.yaml +++ b/src/vf_control/config/parameters.yaml @@ -7,6 +7,8 @@ haptic_control: ff_device_ip_address: "192.168.100.53" ff_device_param_file: "/etc/Haption/Connector/desktop_6D_n65.param" local_ip_address: "192.168.100.50" + haptic_control_rate: 1000.0 + ft_sensor_rate: 500.0 # vf params enable_safety_sphere: false @@ -38,7 +40,7 @@ haptic_control: # vf parameters: # ros__parameters: - mesh_type: "bunny" # knot, sphere, file, bunny + mesh_type: "sphere" # knot, sphere, file, bunny input_mesh_path: "/home/zotac01/Ultrasound-Demo/body_modelling/ros2_ws/final_vf.obj" skin_mesh_path: "/home/zotac01/Ultrasound-Demo/body_modelling/ros2_ws/skin_mesh.obj" output_mesh_path : "/home/zotac01/final_vf2.obj" diff --git a/src/vf_control/include/circular_buffer.hpp b/src/vf_control/include/circular_buffer.hpp new file mode 100644 index 0000000..c317133 --- /dev/null +++ b/src/vf_control/include/circular_buffer.hpp @@ -0,0 +1,105 @@ +#include +#include +#include + +// Template class for Circular Buffer +template +class CircularBuffer { + public: + // Default constructor + CircularBuffer() : capacity_(0), head_(0), tail_(0), size_(0) {} + + // Constructor with capacity + CircularBuffer(size_t capacity) + : capacity_(capacity), buffer_(capacity), head_(0), tail_(0), size_(0) { + if (capacity_ == 0) { + throw std::invalid_argument("Capacity must be greater than zero."); + } + } + + // Initialize buffer with capacity + void initialize(size_t capacity) { + if (capacity == 0) { + throw std::invalid_argument("Capacity must be greater than zero."); + } + capacity_ = capacity; + buffer_.resize(capacity); + head_ = tail_ = size_ = 0; + } + + // Add an element to the buffer + void push(const T& value) { + ensureInitialized(); + buffer_[head_] = value; + head_ = (head_ + 1) % capacity_; + if (size_ == capacity_) { + tail_ = (tail_ + 1) % capacity_; // Overwrite the oldest element + } else { + ++size_; + } + } + + // Remove and return the oldest element + T pop() { + ensureInitialized(); + if (isEmpty()) { + throw std::runtime_error("Buffer is empty."); + } + T value = buffer_[tail_]; + tail_ = (tail_ + 1) % capacity_; + --size_; + return value; + } + + // Get the oldest element without removing it + T peek() const { + ensureInitialized(); + if (isEmpty()) { + throw std::runtime_error("Buffer is empty."); + } + return buffer_[tail_]; + } + + // Check if the buffer is empty + bool isEmpty() const { return size_ == 0; } + + // Check if the buffer is full + bool isFull() const { return size_ == capacity_; } + + // Get the current size of the buffer + size_t size() const { return size_; } + + // Get the capacity of the buffer + size_t capacity() const { return capacity_; } + + // Access an element in the buffer + T& operator[](size_t i) { + ensureInitialized(); + if (i >= size_) { + throw std::out_of_range("Index out of range."); + } + return buffer_[(tail_ + i) % capacity_]; + } + // Access an element in the buffer + const T& operator[](size_t i) const { + ensureInitialized(); + if (i >= size_) { + throw std::out_of_range("Index out of range."); + } + return buffer_[(tail_ + i) % capacity_]; + } + + private: + size_t capacity_; + std::vector buffer_; + size_t head_; + size_t tail_; + size_t size_; + + void ensureInitialized() const { + if (capacity_ == 0) { + throw std::runtime_error( + "Buffer not initialized. Call initialize() first."); + } + } +}; \ No newline at end of file diff --git a/src/vf_control/include/haptic_control_base.hpp b/src/vf_control/include/haptic_control_base.hpp index e513d14..e7d04da 100644 --- a/src/vf_control/include/haptic_control_base.hpp +++ b/src/vf_control/include/haptic_control_base.hpp @@ -20,6 +20,7 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/transform_broadcaster.h" +#include "circular_buffer.hpp" #include "system_interface.hpp" #include "utils.hpp" #include "vf/vf_enforcer.hpp" @@ -41,7 +42,6 @@ class HapticControlBase : public rclcpp::Node { void set_safety_box_width_CB(const rclcpp::Parameter &p); void set_safety_box_length_CB(const rclcpp::Parameter &p); void set_safety_box_height_CB(const rclcpp::Parameter &p); - void update_current_ee_pos(); void store_wrench(const geometry_msgs::msg::WrenchStamped target_wrench); Eigen::Vector3d compute_position_error(); @@ -100,7 +100,16 @@ class HapticControlBase : public rclcpp::Node { bool received_ee_pose_; bool use_limits_; bool enable_safety_sphere_, enable_safety_box_; + // delay simulation double delay_; + int delay_loop_haptic_, delay_loop_ft_; + double haptic_control_rate_,ft_sensor_rate_; +// std::vector wrench_history_buffer_; + // std::vector target_pose_history_buffer_, + // target_pose_vf_history_buffer_; + CircularBuffer wrench_buffer_; + CircularBuffer target_pose_buffer_, + target_pose_vf_buffer_; // Storage for virtuose_node status std::array cur_pose_; diff --git a/src/vf_control/include/system_interface.hpp b/src/vf_control/include/system_interface.hpp index 4c2838b..87acf6e 100644 --- a/src/vf_control/include/system_interface.hpp +++ b/src/vf_control/include/system_interface.hpp @@ -92,6 +92,8 @@ class SystemInterface : public rclcpp::Node { "Failed to call service impedance, client__id_ is zero!"); return; } + } + void start_force_feedback() { set_target_wrench_timer_ = this->create_wall_timer( 1ms, std::bind(&SystemInterface::apply_target_wrench, this)); } diff --git a/src/vf_control/launch/vf.launch.py b/src/vf_control/launch/vf.launch.py index c945e14..0196389 100644 --- a/src/vf_control/launch/vf.launch.py +++ b/src/vf_control/launch/vf.launch.py @@ -41,8 +41,11 @@ def generate_launch_description(): # haptic_wrapper haptic_wrapper = TimerAction( period=0.0, - actions=[Node(package="haption_raptor_api", executable="raptor_api_wrapper",prefix=['xterm -fa "Monospace" -fs 14 -e gdb -ex run --args']) -], + actions=[Node(package="haption_raptor_api", + executable="raptor_api_wrapper", + # prefix=['xterm -fa "Monospace" -fs 14 -e gdb -ex run --args'] + ) + ], ) haptic_parameters_calibration = ( @@ -77,6 +80,8 @@ def generate_launch_description(): ], remappings=[ ( + + # remember to change ft_sensor_rate in the yaml file "bus0/ft_sensor0/ft_sensor_readings/wrench", "/force_torque_sensor_broadcaster/wrench", ) diff --git a/src/vf_control/src/haptic_control_base.cpp b/src/vf_control/src/haptic_control_base.cpp index 64f2bd0..9e33409 100644 --- a/src/vf_control/src/haptic_control_base.cpp +++ b/src/vf_control/src/haptic_control_base.cpp @@ -1,5 +1,4 @@ #include "haptic_control_base.hpp" - using namespace Eigen; using std::placeholders::_1; @@ -36,6 +35,7 @@ HapticControlBase::HapticControlBase(const std::string &name, // safety box dimension this->enable_safety_box_ = this->get_parameter("enable_safety_box").as_bool(); if (this->enable_safety_box_) { + // not implemented yet this->safety_box_width_ = this->get_parameter("safety_box_width").as_double(); // x this->safety_box_length_ = @@ -49,19 +49,31 @@ HapticControlBase::HapticControlBase(const std::string &name, } this->get_parameter("max_force", max_force_); - // safety XYZ position zone -> read from config file this->force_scale_ = this->get_parameter("force_scale").as_double(); this->tool_link_name_ = this->get_parameter("tool_link_name").as_string(); this->base_link_name_ = this->get_parameter("base_link_name").as_string(); this->ft_link_name_ = this->get_parameter("ft_link_name").as_string(); + this->haptic_control_rate_ = + this->get_parameter("haptic_control_rate").as_double(); + this->ft_sensor_rate_ = this->get_parameter("ft_sensor_rate").as_double(); // delay simulation + delay_loop_haptic_ = delay_loop_ft_ = 1; this->delay_ = this->get_parameter("delay").as_double(); this->delay_ = std::clamp(this->delay_, 0.0, 10.0); if (std::abs(this->delay_) > 1e-6) { - RCLCPP_WARN(this->get_logger(), "A Delay of %f seconds is set", - this->delay_); + // compute the number of control loop of delay (haptic run at 1kHz) + this->delay_loop_haptic_ = + (int)std::ceil((this->delay_) * haptic_control_rate_); + this->delay_loop_ft_ = (int)std::ceil((this->delay_) * ft_sensor_rate_); + RCLCPP_WARN(this->get_logger(), + "A Delay of %f seconds is set, which cooresponds to a %d " + "control loop delay", + this->delay_, this->delay_loop_haptic_); } + wrench_buffer_.initialize(delay_loop_ft_); + target_pose_buffer_.initialize(delay_loop_haptic_); + target_pose_vf_buffer_.initialize(delay_loop_haptic_); // Create a parameter subscriber that can be used to monitor parameter changes param_subscriber_ = std::make_shared(this); @@ -125,9 +137,9 @@ HapticControlBase::HapticControlBase(const std::string &name, haptic_device_ = std::make_shared(q_haptic_base_to_robot_base_); - // read new pose + // new pose x_new_ << 0.0, 0.0, 0.0; - // read old pose + // old pose x_old_ << 0.0, 0.0, 0.0; // safe new pose x_tilde_new_ << 0.0, 0.0, 0.0; @@ -176,7 +188,7 @@ void HapticControlBase::set_safety_box_height_CB(const rclcpp::Parameter &p) { void HapticControlBase::store_wrench( const geometry_msgs::msg::WrenchStamped target_wrench) { current_wrench_.header.stamp = target_wrench.header.stamp; - + geometry_msgs::msg::WrenchStamped wrench_stamped; wrench_stamped.header.stamp = target_wrench.header.stamp; wrench_stamped.wrench = target_wrench.wrench; @@ -193,8 +205,8 @@ void HapticControlBase::store_wrench( "F/T sensor pose transform not available: %s", ex.what()); current_wrench_.wrench = geometry_msgs::msg::Wrench(); - return; } + wrench_buffer_.push(current_wrench_); } void HapticControlBase::get_ee_trans( @@ -247,17 +259,28 @@ void HapticControlBase::initialize_haptic_control() { "Waiting for the haptic device to start publishing"); rclcpp::spin_some(haptic_device_); } + haptic_device_->start_force_feedback(); x_new_ << haptic_device_->haptic_starting_pose_.pose.position.x, haptic_device_->haptic_starting_pose_.pose.position.y, haptic_device_->haptic_starting_pose_.pose.position.z; x_old_ = x_tilde_old_ = x_new_; + // in the case of no delay, the buffer will contain only the last element + for (int i = 0; i < delay_loop_haptic_; i++) { + target_pose_buffer_.push(ee_starting_position); + target_pose_vf_buffer_.push(ee_starting_position); + } + for (int i = 0; i < delay_loop_ft_; i++) { + wrench_buffer_.push(current_wrench_); + } + if (use_fixtures_) { init_vf_enforcer(); } - // Perform impedance loop at 1000 Hz + // Perform impedance loop at haptic_control_rate Hz control_thread_ = this->create_wall_timer( - 1ms, std::bind(&HapticControlBase::control_thread, this)); + std::chrono::duration(1.0 / haptic_control_rate_), + std::bind(&HapticControlBase::control_thread, this)); RCLCPP_INFO(this->get_logger(), "\033[0;32mControl thread started\033[0m"); } @@ -305,8 +328,8 @@ void HapticControlBase::control_thread() { x_tilde_new_ = x_new_; } // Applies the feedback force - haptic_device_->update_target_wrench(current_wrench_); - + // retrieve the oldest wrench in the buffer + haptic_device_->update_target_wrench(wrench_buffer_.peek()); // Computes errors Eigen::Vector3d position_error = compute_position_error(); Eigen::Quaterniond orientation_error = compute_orientation_error(); @@ -353,6 +376,8 @@ void HapticControlBase::control_thread() { x_tilde_old_ = x_tilde_new_; } } + target_pose_buffer_.push(target_pose_); + if (use_fixtures_) { Eigen::Vector3d x_desired(target_pose_.pose.position.x, target_pose_.pose.position.y, @@ -370,12 +395,9 @@ void HapticControlBase::control_thread() { target_pose_vf_.pose.orientation = target_pose_.pose.orientation; old_target_pose_vf_ = target_pose_vf_; - } - // RCLCPP_INFO_THROTTLE( - // this->get_logger(), *this->get_clock(), 100, - // "Target position: x: %f | y: %f | z: %f", target_pose_.pose.position.x, - // target_pose_.pose.position.y, target_pose_.pose.position.z); + target_pose_vf_buffer_.push(target_pose_vf_); + } // Publish the current ee pose geometry_msgs::msg::TransformStamped ee_pose; @@ -386,16 +408,13 @@ void HapticControlBase::control_thread() { ee_pose_msg.pose.position = vector3_to_point(ee_pose.transform.translation); ee_pose_msg.pose.orientation = ee_pose.transform.rotation; current_frame_pub_->publish(ee_pose_msg); - if (this->use_fixtures_) { // Publish the target pose - target_frame_pub_->publish(target_pose_vf_); + target_frame_pub_->publish(target_pose_vf_buffer_.peek()); } else { - // Publish the target pose - target_frame_pub_->publish(target_pose_); + target_frame_pub_->publish(target_pose_buffer_.peek()); } tf_broadcaster_->sendTransform(target_pose_tf_); - // Publish the desired pose desired_frame_pub_->publish(target_pose_);