Skip to content

Commit

Permalink
Merge pull request #1 from idra-lab/separated_interface
Browse files Browse the repository at this point in the history
Separated interface
  • Loading branch information
Hydran00 authored Dec 12, 2024
2 parents df7f441 + f1cf2d2 commit 5a3e5ce
Show file tree
Hide file tree
Showing 14 changed files with 532 additions and 356 deletions.
20 changes: 11 additions & 9 deletions src/haption_raptor_api/src/raptor_api_wrapper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -120,7 +120,7 @@ class RaptorAPIWrapper : public rclcpp::Node {
}
// Watchdog on receiving pose setpoint
if ((_now - last_loop_date) > 30000000) {
RCLCPP_INFO(rclcpp::get_logger("rclcpp"),
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),
"Timeout on set pose: %ld, going to state READY",
_now - last_loop_date);
Reset();
Expand Down Expand Up @@ -153,7 +153,7 @@ class RaptorAPIWrapper : public rclcpp::Node {
}
// Watchdog on receiving speed setpoint
if ((_now - last_loop_date) > 30000000) {
RCLCPP_INFO(rclcpp::get_logger("rclcpp"),
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),
"Timeout on set speed: %ld, going to state READY",
_now - last_loop_date);
Reset();
Expand All @@ -179,14 +179,14 @@ class RaptorAPIWrapper : public rclcpp::Node {
last_in_date = _now;

if (ctr % 1000 == 0) {
RCLCPP_INFO(rclcpp::get_logger("rclcpp"),
"Force set point: %f %f %f %f %f %f", set_force[0],
set_force[1], set_force[2], set_force[3], set_force[4],
set_force[5]);
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"),
// "Force set point: %f %f %f %f %f %f", set_force[0],
// set_force[1], set_force[2], set_force[3], set_force[4],
// set_force[5]);
}
// Watchdog on receiving force setpoint
if ((_now - last_loop_date) > 30000000) {
RCLCPP_INFO(rclcpp::get_logger("rclcpp"),
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),
"Timeout on set force: %ld, going to state READY",
_now - last_loop_date);
Reset();
Expand Down Expand Up @@ -288,6 +288,8 @@ class RaptorAPIWrapper : public rclcpp::Node {
break;
case HAPTION::CalibrationStatus::C_IDLE:
break;
default:
break;
}
}
// Update status
Expand Down Expand Up @@ -910,8 +912,8 @@ class RaptorAPIWrapper : public rclcpp::Node {
// Watchdog on receiving force setpoint
if (isFirstSetpointReceived &&
abs((int64_t)last_in_date - (int64_t)last_loop_date) > 30000000) {
RCLCPP_INFO(rclcpp::get_logger("rclcpp"),
"Timeout on set force: %ld, going to state READY",
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),
"Impedance thread: Timeout on set force: %ld, going to state READY",
last_in_date - last_loop_date);
Reset();
return;
Expand Down
4 changes: 2 additions & 2 deletions src/test_admittance/src/test_admittance.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -149,7 +149,7 @@ class TestAdmittance : public rclcpp::Node {
rclcpp::FutureReturnCode::SUCCESS) {
// Store client ID given by virtuose_node
client_id = result.get()->client_id;
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Our client ID is: %ld",
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Our client ID is: %d",
client_id);
} else {
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),
Expand Down Expand Up @@ -206,7 +206,7 @@ class TestAdmittance : public rclcpp::Node {
(long long unsigned int)status_date_nanosec;
// Print status every second
if (ctr % 1000 == 0) {
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Status: %llu %d %d %f %f %f",
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Status: %lu %d %d %f %f %f",
dt, status_state, status_button, cur_pose[0], cur_pose[1],
cur_pose[2]);
}
Expand Down
8 changes: 4 additions & 4 deletions src/test_impedance/src/test_impedance.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -123,7 +123,7 @@ class TestImpedance : public rclcpp::Node {
rclcpp::FutureReturnCode::SUCCESS) {
// Store client ID given by virtuose_node
client_id = result.get()->client_id;
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Our client ID is: %ld",
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Our client ID is: %d",
client_id);
} else {
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),
Expand Down Expand Up @@ -167,9 +167,9 @@ class TestImpedance : public rclcpp::Node {
}
_in_virtuose_force->publish(force);
ctr++;
uint64_t dt = get_clock()->now().nanoseconds() -
(long long unsigned int)status_date_sec * 1000000000U +
(long long unsigned int)status_date_nanosec;
// uint64_t dt = get_clock()->now().nanoseconds() -
// (long long unsigned int)status_date_sec * 1000000000U +
// (long long unsigned int)status_date_nanosec;
// Print status every second
if (ctr % 1000 == 0) {
// RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Status: %llu %d %d %f %f
Expand Down
6 changes: 4 additions & 2 deletions 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,10 +40,10 @@ haptic_control:

# vf parameters:
# ros__parameters:
mesh_type: "file" # 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/Ultrasound-Demo/body_modelling/ros2_ws/final_vf2.obj"
output_mesh_path : "/home/zotac01/virtual_fixture_vis.obj"
radius: 0.005
lookup_area: 0.006 # should be at least 2*radius
plane_size: 0.001 # plane size used to visualize active constraints in rviz
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.");
}
}
};
63 changes: 24 additions & 39 deletions src/vf_control/include/haptic_control_base.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,20 +15,13 @@

#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/wrench_stamped.hpp"
#include "raptor_api_interfaces/msg/in_virtuose_force.hpp"
#include "raptor_api_interfaces/msg/in_virtuose_pose.hpp"
#include "raptor_api_interfaces/msg/in_virtuose_speed.hpp"
#include "raptor_api_interfaces/msg/out_virtuose_force.hpp"
#include "raptor_api_interfaces/msg/out_virtuose_physical_pose.hpp"
#include "raptor_api_interfaces/msg/out_virtuose_pose.hpp"
#include "raptor_api_interfaces/msg/out_virtuose_speed.hpp"
#include "raptor_api_interfaces/msg/out_virtuose_status.hpp"
#include "raptor_api_interfaces/srv/virtuose_impedance.hpp"
#include "raptor_api_interfaces/srv/virtuose_reset.hpp"

#include "tf2/LinearMath/Quaternion.h"
#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,20 +34,20 @@ class HapticControlBase : public rclcpp::Node {
rclcpp::NodeOptions()
.allow_undeclared_parameters(true)
.automatically_declare_parameters_from_overrides(true));
// safe sphere virtual fixture
void enable_safety_sphere_CB(const rclcpp::Parameter &p);
void set_safety_sphere_radius_CB(const rclcpp::Parameter &p);
// safe box virtual fixture
void enable_safety_box_CB(const rclcpp::Parameter &p);
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 set_wrench(const geometry_msgs::msg::WrenchStamped target_wrench);
void out_virtuose_pose_CB(
const raptor_api_interfaces::msg::OutVirtuosePose::SharedPtr msg);
void out_virtuose_statusCB(
const raptor_api_interfaces::msg::OutVirtuoseStatus::SharedPtr msg);
void call_impedance_service();
void impedance_thread();
void store_wrench(const geometry_msgs::msg::WrenchStamped target_wrench);
Eigen::Vector3d compute_position_error();
Eigen::Quaterniond compute_orientation_error();
void initialize_haptic_control();
void control_thread();
void get_ee_trans(geometry_msgs::msg::TransformStamped &trans);
void init_vf_enforcer();
void project_target_on_sphere(Eigen::Vector3d &target_position_vec,
Expand All @@ -64,31 +57,20 @@ class HapticControlBase : public rclcpp::Node {
old_target_pose_vf_;
std::string base_link_name_;
bool use_fixtures_ = true;
std::shared_ptr<SystemInterface> haptic_device_;

private:
// ROS2 subscribtions
rclcpp::Subscription<raptor_api_interfaces::msg::OutVirtuoseStatus>::SharedPtr
out_virtuose_status_;
rclcpp::Subscription<raptor_api_interfaces::msg::OutVirtuosePose>::SharedPtr
_out_virtuose_pose_;
rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr
ft_subscriber_;
// ROS2 publishers
rclcpp::Publisher<raptor_api_interfaces::msg::InVirtuoseForce>::SharedPtr
_in_virtuose_force;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr
target_frame_pub_;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr
current_frame_pub_;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr
desired_frame_pub_;

// ROS2 service client_s
rclcpp::Client<raptor_api_interfaces::srv::VirtuoseImpedance>::SharedPtr
client_;
// ROS2 timers
rclcpp::TimerBase::SharedPtr pose_update_timer_;
rclcpp::TimerBase::SharedPtr impedanceThread_;
// ROS2 tf2 transform listener and buffer
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
Expand All @@ -106,9 +88,8 @@ class HapticControlBase : public rclcpp::Node {
geometry_msgs::msg::PoseStamped old_pose_;
geometry_msgs::msg::PoseStamped ee_starting_position;
geometry_msgs::msg::PoseStamped ee_current_pose_;
geometry_msgs::msg::PoseStamped haptic_starting_position_;
Eigen::Quaterniond q_haptic_base_to_robot_base_;
raptor_api_interfaces::msg::InVirtuoseForce old_force_;
geometry_msgs::msg::PoseStamped haptic_starting_pose_;
Eigen::Quaterniond q_haptic_base_to_robot_base_, qEEStart;
double safety_sphere_radius_, safety_box_width_, safety_box_length_,
safety_box_height_;
double max_force_;
Expand All @@ -119,16 +100,18 @@ 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
int64_t status_date_sec_;
uint32_t status_date_nanosec_;
int client__id_;
int ctr_;
// Storage for virtuose pose
int64_t pose_date_sec_;
uint32_t pose_date_nanosec_;
std::array<double, 7> cur_pose_;
std::array<double, 3> bounding_box_center_;
uint32_t status_state_;
Expand All @@ -139,6 +122,8 @@ class HapticControlBase : public rclcpp::Node {
Eigen::Vector3d x_tilde_old_, x_tilde_new_;

std::shared_ptr<VFEnforcer> vf_enforcer_;
// ros control_thread_
rclcpp::TimerBase::SharedPtr control_thread_;
};

#endif // __HAPTIC_CONTROL_BASE__
15 changes: 0 additions & 15 deletions src/vf_control/include/system_interface.cpp

This file was deleted.

Loading

0 comments on commit 5a3e5ce

Please sign in to comment.