Skip to content

Commit

Permalink
solved timing bugs
Browse files Browse the repository at this point in the history
  • Loading branch information
Hydran00 committed Dec 11, 2024
1 parent bea2b28 commit ccdfb89
Show file tree
Hide file tree
Showing 7 changed files with 170 additions and 26 deletions.
2 changes: 2 additions & 0 deletions src/haption_raptor_api/src/raptor_api_wrapper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -288,6 +288,8 @@ class RaptorAPIWrapper : public rclcpp::Node {
break;
case HAPTION::CalibrationStatus::C_IDLE:
break;
default:
break;
}
}
// Update status
Expand Down
4 changes: 3 additions & 1 deletion src/vf_control/config/parameters.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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"
Expand Down
105 changes: 105 additions & 0 deletions src/vf_control/include/circular_buffer.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,105 @@
#include <iostream>
#include <stdexcept>
#include <vector>

// Template class for Circular Buffer
template <typename T>
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<T> 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.");
}
}
};
11 changes: 10 additions & 1 deletion src/vf_control/include/haptic_control_base.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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();
Expand Down Expand Up @@ -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<geometry_msgs::msg::WrenchStamped> wrench_history_buffer_;
// std::vector<geometry_msgs::msg::PoseStamped> target_pose_history_buffer_,
// target_pose_vf_history_buffer_;
CircularBuffer<geometry_msgs::msg::WrenchStamped> wrench_buffer_;
CircularBuffer<geometry_msgs::msg::PoseStamped> target_pose_buffer_,
target_pose_vf_buffer_;

// Storage for virtuose_node status
std::array<double, 7> cur_pose_;
Expand Down
2 changes: 2 additions & 0 deletions src/vf_control/include/system_interface.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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));
}
Expand Down
9 changes: 7 additions & 2 deletions src/vf_control/launch/vf.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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 = (
Expand Down Expand Up @@ -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",
)
Expand Down
63 changes: 41 additions & 22 deletions src/vf_control/src/haptic_control_base.cpp
Original file line number Diff line number Diff line change
@@ -1,5 +1,4 @@
#include "haptic_control_base.hpp"

using namespace Eigen;

using std::placeholders::_1;
Expand Down Expand Up @@ -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_ =
Expand All @@ -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<rclcpp::ParameterEventHandler>(this);
Expand Down Expand Up @@ -125,9 +137,9 @@ HapticControlBase::HapticControlBase(const std::string &name,
haptic_device_ =
std::make_shared<SystemInterface>(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;
Expand Down Expand Up @@ -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;
Expand All @@ -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(
Expand Down Expand Up @@ -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<double>(1.0 / haptic_control_rate_),
std::bind(&HapticControlBase::control_thread, this));
RCLCPP_INFO(this->get_logger(), "\033[0;32mControl thread started\033[0m");
}

Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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,
Expand All @@ -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;
Expand All @@ -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_);

Expand Down

0 comments on commit ccdfb89

Please sign in to comment.