diff --git a/src/haptic_control/config/parameters.yaml b/src/haptic_control/config/haptic_parameters.yaml similarity index 90% rename from src/haptic_control/config/parameters.yaml rename to src/haptic_control/config/haptic_parameters.yaml index c2a3f28..8bfa008 100755 --- a/src/haptic_control/config/parameters.yaml +++ b/src/haptic_control/config/haptic_parameters.yaml @@ -5,7 +5,6 @@ haptic_control: 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 max_force: 6.0 # Newton enable_safety_sphere: false @@ -23,9 +22,7 @@ haptic_control: # min_z_: -0.10 # max_z_: 0.13 - tool_link_name: "probe" - base_link_name: "base_link" - ft_link_name: "tool0" + delay: 0.0 # round trip simulated delay in seconds (0.0 for no delay) force_scale : 0.3 # safety box that can be activated (centered in the current ee position) -> Not implemented yet diff --git a/src/haptic_control/config/kuka_parameters.yaml b/src/haptic_control/config/kuka_parameters.yaml new file mode 100755 index 0000000..9d0e364 --- /dev/null +++ b/src/haptic_control/config/kuka_parameters.yaml @@ -0,0 +1,7 @@ +haptic_control: + ros__parameters: + tool_link_name: "lbr_link_ee" + base_link_name: "lbr_link_0" + ft_link_name: "lbr_link_ee" + ft_sensor_rate: 1000.0 + ft_topic_name: "/todo" \ No newline at end of file diff --git a/src/haptic_control/config/ur3e_parameters.yaml b/src/haptic_control/config/ur3e_parameters.yaml new file mode 100644 index 0000000..dd6196b --- /dev/null +++ b/src/haptic_control/config/ur3e_parameters.yaml @@ -0,0 +1,8 @@ +haptic_control: + ros__parameters: + tool_link_name: "probe" + base_link_name: "base_link" + ft_link_name: "tool0" + ft_sensor_rate: 500.0 + ft_topic_name: "/force_torque_sensor_broadcaster/wrench" # UR f/t sensor + # ft_topic_name: "bus0/ft_sensor0/ft_sensor_readings/wrench" -> Bota f/t sensor \ No newline at end of file diff --git a/src/haptic_control/include/haptic_control_base.hpp b/src/haptic_control/include/haptic_control_base.hpp index b1177c6..2a45063 100644 --- a/src/haptic_control/include/haptic_control_base.hpp +++ b/src/haptic_control/include/haptic_control_base.hpp @@ -101,6 +101,7 @@ class HapticControlBase : public rclcpp::Node { double force_scale_; std::string tool_link_name_; std::string ft_link_name_; + std::string ft_topic_name_; bool received_haptic_pose_; bool received_ee_pose_; bool enable_safety_sphere_, enable_safety_box_; diff --git a/src/haptic_control/include/sample_teleoperation.hpp b/src/haptic_control/include/sample_teleoperation.hpp index fc3640d..838cb53 100644 --- a/src/haptic_control/include/sample_teleoperation.hpp +++ b/src/haptic_control/include/sample_teleoperation.hpp @@ -115,6 +115,7 @@ class HapticControl : public rclcpp::Node { std::string tool_link_name_; std::string base_link_name_; std::string ft_link_name_; + std::string ft_topic_name_; bool received_haptic_pose_; bool received_ee_pose_; bool enable_safety_sphere_, enable_safety_box_; diff --git a/src/haptic_control/launch/haptic_control.launch.py b/src/haptic_control/launch/haptic_control.launch.py index ff9afb5..fbc9a5b 100644 --- a/src/haptic_control/launch/haptic_control.launch.py +++ b/src/haptic_control/launch/haptic_control.launch.py @@ -5,31 +5,24 @@ from launch.actions import TimerAction, DeclareLaunchArgument from ament_index_python.packages import get_package_share_directory from launch.actions import DeclareLaunchArgument - +from launch.actions import DeclareLaunchArgument, OpaqueFunction # PythonExpression from launch.conditions import LaunchConfigurationEquals -def generate_launch_description(): - ld = LaunchDescription() - # LOGINFO - ld.add_action( - DeclareLaunchArgument( - "use_fixtures", - default_value="false", - description="Use this argument to activate the fixtures (true or false)", - ) - ) - ld.add_action( - DeclareLaunchArgument( - "delay", - default_value="0.0", - description="Use this argument to simulate a delay in the system (in seconds), use 0.0 for no delay", - ) - ) +launch_args = [ + DeclareLaunchArgument("delay", default_value="0.0", description="Use this argument to simulate a delay in the system (in seconds), use 0.0 for no delay"), + DeclareLaunchArgument("use_fixtures", default_value="false", description="Use this argument to activate the fixtures (true or false)"), + DeclareLaunchArgument("robot_type", default_value="ur3e", description="Use this argument to activate the fixtures (true or false)"), +] +def launch_setup(context): + # SET RIGHT PATH TO YAML + haptic_params = get_package_share_directory("haptic_control") + "/config/haptic_parameters.yaml" + robot_params = get_package_share_directory("haptic_control") + "/config/" + LaunchConfiguration('robot_type').perform(context) + "_parameters.yaml" + # haptic_wrapper haptic_wrapper = TimerAction( period=0.0, @@ -39,9 +32,7 @@ def generate_launch_description(): ) ], ) - - # SET RIGHT PATH TO YAML - params = get_package_share_directory("haptic_control") + "/config/parameters.yaml" + vf_node = TimerAction( period=2.5, actions=[ @@ -50,7 +41,7 @@ def generate_launch_description(): executable="haptic_control", # remappings=[('/target_frame', '/target_frame_haptic')], parameters=[ - ParameterFile(params), + ParameterFile(haptic_params), ParameterFile(robot_params), {"use_fixtures": LaunchConfiguration("use_fixtures")}, {"delay": LaunchConfiguration("delay")}, ], @@ -78,7 +69,14 @@ def generate_launch_description(): arguments=["-d", get_package_share_directory("haptic_control") + "/rviz/vf.rviz"], condition=LaunchConfigurationEquals("use_fixtures", "true"), ) - ld.add_action(haptic_wrapper) - ld.add_action(vf_node) - ld.add_action(rviz) + return [haptic_wrapper, vf_node, rviz] + + + +def generate_launch_description(): + ld = LaunchDescription(launch_args) + opfunc = OpaqueFunction(function = launch_setup) + ld.add_action(opfunc) + print("A") + return ld diff --git a/src/haptic_control/launch/sample_teleoperation.launch.py b/src/haptic_control/launch/sample_teleoperation.launch.py index c5f8a5d..39fe6b3 100644 --- a/src/haptic_control/launch/sample_teleoperation.launch.py +++ b/src/haptic_control/launch/sample_teleoperation.launch.py @@ -27,7 +27,8 @@ def generate_launch_description(): ) # SET RIGHT PATH TO YAML - params = get_package_share_directory("haptic_control") + "/config/parameters.yaml" + haptic_params = get_package_share_directory("haptic_control") + "/config/haptic_parameters.yaml" + robot_params = get_package_share_directory("haptic_control") + "/config/ur3e_parameters.yaml" sample_teleoperation = TimerAction( period=1.0, @@ -37,11 +38,11 @@ def generate_launch_description(): executable="sample_teleoperation", # remappings=[('/target_frame', '/target_frame_haptic')], parameters=[ - ParameterFile(params), + ParameterFile(haptic_params), ParameterFile(robot_params) ], remappings=[ ( - # remember to change ft_sensor_rate in the yaml file + # remember to set the right ft_sensor_rate in the haptic yaml file "bus0/ft_sensor0/ft_sensor_readings/wrench", # 1 kHz "/force_torque_sensor_broadcaster/wrench", # 0.5 kHz ) diff --git a/src/haptic_control/src/haptic_control_base.cpp b/src/haptic_control/src/haptic_control_base.cpp index 8d39275..fc03f4c 100644 --- a/src/haptic_control/src/haptic_control_base.cpp +++ b/src/haptic_control/src/haptic_control_base.cpp @@ -53,7 +53,7 @@ HapticControlBase::HapticControlBase(const std::string &name, this->safety_box_length_ = std::numeric_limits::infinity(); this->safety_box_height_ = std::numeric_limits::infinity(); } - + this->get_parameter("ft_topic_name", ft_topic_name_); this->get_parameter("max_force", max_force_); this->force_scale_ = this->get_parameter("force_scale").as_double(); this->tool_link_name_ = this->get_parameter("tool_link_name").as_string(); @@ -103,8 +103,7 @@ HapticControlBase::HapticControlBase(const std::string &name, // create force/wrench subscriber ft_subscriber_ = this->create_subscription( - "bus0/ft_sensor0/ft_sensor_readings/wrench", 1, - std::bind(&HapticControlBase::store_wrench, this, _1)); + ft_topic_name_, 1, std::bind(&HapticControlBase::store_wrench, this, _1)); // Set a callback for parameters updates // Safety sphere @@ -140,6 +139,12 @@ HapticControlBase::HapticControlBase(const std::string &name, // defines the rotation from the robot base frame to the haptic base frame Eigen::Quaterniond q_haptic_base_to_robot_base_( Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ())); + RCLCPP_INFO( + this->get_logger(), + "Haptic base to robot base rotation: x: %f | y: %f | z: %f | w: " + "%f", + q_haptic_base_to_robot_base_.x(), q_haptic_base_to_robot_base_.y(), + q_haptic_base_to_robot_base_.z(), q_haptic_base_to_robot_base_.w()); haptic_device_ = std::make_shared(q_haptic_base_to_robot_base_); diff --git a/src/haptic_control/src/sample_teleoperation.cpp b/src/haptic_control/src/sample_teleoperation.cpp index 0ec07c1..67ba952 100644 --- a/src/haptic_control/src/sample_teleoperation.cpp +++ b/src/haptic_control/src/sample_teleoperation.cpp @@ -38,7 +38,7 @@ HapticControl::HapticControl(const std::string &name, this->safety_box_length_ = std::numeric_limits::infinity(); this->safety_box_height_ = std::numeric_limits::infinity(); } - + this->get_parameter("ft_topic_name", ft_topic_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(); @@ -80,8 +80,7 @@ HapticControl::HapticControl(const std::string &name, "in_virtuose_force", 1); // create force/wrench subscriber ft_subscriber_ = this->create_subscription( - "bus0/ft_sensor0/ft_sensor_readings/wrench", 1, - std::bind(&HapticControl::SetWrenchCB, this, _1)); + ft_topic_name_, 1, std::bind(&HapticControl::SetWrenchCB, this, _1)); impedance_client_ = this->create_client( @@ -415,13 +414,13 @@ void HapticControl::impedanceThread() { float alpha = 0; force.virtuose_force.force.x = force_scale_ * (alpha * old_force_.virtuose_force.force.x + - (1 - alpha) * current_wrench_.wrench.force.x); + (1 - alpha) * current_wrench_.wrench.force.x); force.virtuose_force.force.y = force_scale_ * (alpha * old_force_.virtuose_force.force.y + - (1 - alpha) * current_wrench_.wrench.force.y); + (1 - alpha) * current_wrench_.wrench.force.y); force.virtuose_force.force.z = force_scale_ * (alpha * old_force_.virtuose_force.force.z + - (1 - alpha) * current_wrench_.wrench.force.z); + (1 - alpha) * current_wrench_.wrench.force.z); // torque omitted for control simplicity force.virtuose_force.torque.x = 0.0; // 0.2 * (alpha * old_force_.virtuose_force.torque.x + (1 - alpha)