From 79d1eec1379241965bacbe0a742e68f636eada9e Mon Sep 17 00:00:00 2001 From: Niko Date: Tue, 16 Apr 2024 18:22:42 +0200 Subject: [PATCH] Forward Controller working Now 2 states where published: gripper_gap - which is the actuall gap between the two finger plates finger_joint - which is the old angle --- .../robotiq_forward_controller_old.hpp | 47 ------------------- .../config/robotiq_2f_140_config.yaml | 2 +- .../MockRobotiq2fSocketAdapter.hpp | 6 +-- 3 files changed, 3 insertions(+), 52 deletions(-) delete mode 100644 src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_forward_controller_old.hpp diff --git a/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_forward_controller_old.hpp b/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_forward_controller_old.hpp deleted file mode 100644 index 52bcddf..0000000 --- a/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_forward_controller_old.hpp +++ /dev/null @@ -1,47 +0,0 @@ - -#ifndef ROBOTIQ_FORWARD_CONTROLLER_HPP -#define ROBOTIQ_FORWARD_CONTROLLER_HPP - -#include -#include - -#include "controller_interface/controller_interface.hpp" -#include -#include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "hardware_interface/loaned_command_interface.hpp" -#include - -#include "robotiq_2f_msgs/msg/forward_command.hpp" - - -namespace robotiq_2f_controllers -{ -class RobotiqForwardController : public controller_interface::ControllerInterface -{ -public: - RobotiqForwardController(); - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; - - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; - -private: - std::string joint_name_; - hardware_interface::LoanedCommandInterface position_command_; - hardware_interface::LoanedCommandInterface max_effort_command_; - hardware_interface::LoanedCommandInterface max_velocity_command_; - - rclcpp::Subscription::SharedPtr command_subscriber_; - double desired_position_; - double desired_max_velocity_; - double desired_max_effort_; - - void command_callback(const robotiq_2f_msgs::msg::ForwardCommand::SharedPtr msg); - -}; -} -#endif diff --git a/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml b/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml index fb445f4..8c1cf5f 100644 --- a/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml +++ b/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml @@ -4,6 +4,6 @@ robotiq_2f_gripper: max_position: 0.14 # Meters (fully open) max_angle: 0.695 # radiants (closed) min_speed: 0.02 # Meters per second - max_speed: 0.5 # Meters per second + max_speed: 0.15 # Meters per second min_force: 20.0 # Newtons max_force: 235.0 # Newtons \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp index a3f4288..ae3ae9e 100644 --- a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp +++ b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp @@ -51,9 +51,7 @@ public: isActive.store(true); // Ensure we set active before moving if (!autoCalibrate) { // Simulate a calibration sequence - move_and_wait(0, 64, 1); - move_and_wait(255, 64, 1); - move_and_wait(0, 64, 1); + move_and_wait(0, 255, 1); } } @@ -96,7 +94,7 @@ private: // Simulate moving towards the target position while (gripperPosition.load() != targetPosition) { gripperPosition.fetch_add(step); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Simulate time to move a step + std::this_thread::sleep_for(std::chrono::milliseconds(10)); // Simulate time to move a step } std::cout << "Reached position " << targetPosition << " with speed " << speed << " and force " << force << std::endl;