Niko/UR_Robotiq
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Forward Controller working

Browse Source 
But its not using the Gap as Control Value but the angles
This commit is contained in:
Niko Feith 2024-04-16 15:42:43 +02:00
parent 95cf345de4
commit 5b86af00da
9 changed files with 68 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt
View File

@ -15,7 +15,7 @@ find_package(realtime_tools REQUIRED)
include_directories(include)
add_library(${PROJECT_NAME}
add_library(robotiq_2f_controllers SHARED
src/robotiq_forward_controller.cpp
)
ament_target_dependencies(${PROJECT_NAME}

1
src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_forward_controller.hpp
View File

@ -68,6 +68,7 @@ public:
protected:
std::string joint_name_;
std::vector<std::string> command_interface_types_;
std::vector<std::string> state_interface_types_;
// communication interface
bool new_msg_ = false;

72
src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp
View File

@ -11,23 +11,43 @@ controller_interface::CallbackReturn RobotiqForwardController::on_init()
{
joint_name_ = auto_declare<std::string>("joint", joint_name_);
command_interface_types_ =
auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
state_interface_types_ =
auto_declare<std::vector<std::string>>("state_interfaces", state_interface_types_);
if (joint_name_.empty()) {
RCLCPP_ERROR(get_node()->get_logger(), "Missing 'joint_name' parameter.");
return CallbackReturn::ERROR;
}
else{
RCLCPP_INFO(get_node()->get_logger(), "Joint name: %s.", joint_name_.c_str());
}
return CallbackReturn::SUCCESS;
}
controller_interface::InterfaceConfiguration RobotiqForwardController::command_interface_configuration()
const
controller_interface::InterfaceConfiguration RobotiqForwardController::command_interface_configuration() const
{
controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
RCLCPP_DEBUG(get_node()->get_logger(), "Starting configuration of command interfaces.");
conf.names.reserve(command_interface_types_.size());
for (const auto & interface_type : command_interface_types_)
{
conf.names.push_back(joint_name_ + "/" + interface_type);
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto& interface_map_entry : command_interface_map_) {
const std::string& interface_type = interface_map_entry.first;
config.names.push_back(joint_name_ + "/" + interface_type);
}
RCLCPP_DEBUG(get_node()->get_logger(), "Completed configuration of command interfaces.");
return conf;
return config;
}
controller_interface::InterfaceConfiguration RobotiqForwardController::state_interface_configuration() const {
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::NONE; // Or whatever is appropriate
return config;
}
controller_interface::CallbackReturn RobotiqForwardController::on_configure(const rclcpp_lifecycle::State &)
@ -47,18 +67,23 @@ controller_interface::CallbackReturn RobotiqForwardController::on_configure(cons
controller_interface::CallbackReturn RobotiqForwardController::on_activate(const rclcpp_lifecycle::State &)
{
// clear out vectors in case of restart
joint_position_command_interface_.clear();
max_velocity_command_interface_.clear();
max_effort_command_interface_.clear();
RCLCPP_INFO(get_node()->get_logger(), "Available command interfaces:");
for (const auto& interface : command_interfaces_) {
RCLCPP_INFO(get_node()->get_logger(), "Interface Name: %s, Interface Type: %s", interface.get_name().c_str(), interface.get_interface_name().c_str());
}
// assign command interfaces
for (auto & interface : command_interfaces_)
{
command_interface_map_[interface.get_interface_name()]->push_back(interface);
}
// clear out vectors in case of restart
joint_position_command_interface_.clear();
max_velocity_command_interface_.clear();
max_effort_command_interface_.clear();
return CallbackReturn::SUCCESS;
// assign command interfaces
for (auto & interface : command_interfaces_)
{
command_interface_map_[interface.get_interface_name()]->push_back(interface);
}
return CallbackReturn::SUCCESS;
}
controller_interface::return_type RobotiqForwardController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
@ -71,6 +96,11 @@ controller_interface::return_type RobotiqForwardController::update(const rclcpp:
desired_max_effort_ = command_msg_->max_effort;
}
// Set the hardware interface commands
if (joint_position_command_interface_.empty()) {
RCLCPP_FATAL(get_node()->get_logger(), "Command interface is not available.");
return controller_interface::return_type::ERROR;
}
joint_position_command_interface_[0].get().set_value(desired_position_);
max_velocity_command_interface_[0].get().set_value(desired_max_velocity_);
max_effort_command_interface_[0].get().set_value(desired_max_effort_);
@ -101,3 +131,7 @@ controller_interface::CallbackReturn RobotiqForwardController::on_shutdown(const
return CallbackReturn::SUCCESS;
}
}
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(robotiq_2f_controllers::RobotiqForwardController, controller_interface::ControllerInterface)

82
src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller_old.cpp
View File

@ -1,82 +0,0 @@
#include "robotiq_2f_controllers/robotiq_forward_controller.hpp"
#include <pluginlib/class_list_macros.hpp>
namespace robotiq_2f_controllers
{
RobotiqForwardController::RobotiqForwardController()
{
}
controller_interface::InterfaceConfiguration RobotiqForwardController::command_interface_configuration() const
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
config.names.emplace_back(joint_name_ + hardware_interface::HW_IF_POSITION);
config.names.emplace_back(joint_name_ + "/set_gripper_max_effort");
config.names.emplace_back(joint_name_ + "/set_gripper_max_velocity");
return config;
}
controller_interface::InterfaceConfiguration RobotiqForwardController::state_interface_configuration() const
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::NONE;
return config;
}
void RobotiqForwardController::command_callback(const robotiq_2f_msgs::msg::ForwardCommand::SharedPtr msg)
{
desired_position_ = msg->position;
desired_max_velocity_ = msg->max_velocity;
desired_max_effort_ = msg->max_effort;
}
controller_interface::return_type RobotiqForwardController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
{
// Set the hardware interface commands
position_command_.set_value(desired_position_);
max_effort_command_.set_value(desired_max_effort_);
max_velocity_command_.set_value(desired_max_velocity_);
return controller_interface::return_type::OK;
}
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqForwardController::on_init()
{
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqForwardController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
{
joint_name_ = get_node()->declare_parameter<std::string>("joint_name", "finger_joint");
auto command_interfaces = get_node()->get_node_base_interface();
auto position_handle = robot_hardware.get_handle(joint_name_ + hardware_interface::HW_IF_POSITION);
position_command_ = hardware_interface::LoanedCommandInterface(command_interfaces_[0].);
max_effort_command_ = hardware_interface::LoanedCommandInterface(command_interfaces_[0].);
max_velocity_command_ = hardware_interface::LoanedCommandInterface(command_interfaces_(joint_name_ + "/set_gripper_max_velocity"));
command_subscriber_ = get_node()->create_subscription<robotiq_2f_msgs::msg::ForwardCommand>(
"~/command", 10, std::bind(&RobotiqForwardController::command_callback, this, std::placeholders::_1));
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqForwardController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
{
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqForwardController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
{
for (size_t i = 0; i < command_interfaces_.size(); ++i) {
command_interfaces_[i].set_value(std::numeric_limits<double>::quiet_NaN());
}
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
} // namespace robotiq_2f_controllers
PLUGINLIB_EXPORT_CLASS(robotiq_2f_controllers::RobotiqForwardController, controller_interface::ControllerInterface)

4
src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml
View File

@ -1,7 +1,7 @@
robotiq_2f_gripper:
# Physical limits
min_position: 0.0 # Meters (fully closed)
max_position: 0.14 # Meters (fully open)
min_position: 0.0 # radiant (fully open)
max_position: 0.695 # radiant (fully open)
min_speed: 0.02 # Meters per second
max_speed: 0.15 # Meters per second
min_force: 20.0 # Newtons

5
src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml
View File

@ -8,7 +8,7 @@ controller_manager:
fake_gripper_controller:
type: joint_trajectory_controller/JointTrajectoryController
forward_gripper_position_controller:
type: position_controllers/JointGroupPositionController
type: robotiq_2f_controllers/RobotiqForwardController
robotiq_activation_controller:
type: robotiq_controllers/RobotiqActivationController
@ -21,8 +21,7 @@ robotiq_gripper_controller:
forward_gripper_position_controller:
ros__parameters:
joints:
- finger_joint
joint: finger_joint
fake_gripper_controller:
ros__parameters:

6
src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py
View File

@ -122,9 +122,9 @@ def generate_launch_description():
controller_spawner_inactive_names = ["forward_gripper_position_controller",
"fake_gripper_controller"]
else:
controller_spawner_names = ["fake_gripper_controller"]
controller_spawner_inactive_names = ["forward_gripper_position_controller",
"robotiq_gripper_controller"]
controller_spawner_names = ["forward_gripper_position_controller"]
controller_spawner_inactive_names = ["robotiq_gripper_controller",
"fake_gripper_controller"]
controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [
controller_spawner(name, active=False) for name in controller_spawner_inactive_names

3
src/robotiq_2f/robotiq_2f_description/package.xml
View File

@ -17,9 +17,12 @@
<exec_depend>urdf</exec_depend>
<exec_depend>xacro</exec_depend>
<exec_depend>robotiq_2f_msgs</exec_depend>
<exec_depend>ros2_control</exec_depend>
<exec_depend>ros2_controllers</exec_depend>
<exec_depend>robotiq_2f_controllers</exec_depend>
<exec_depend>gripper_controllers</exec_depend>
<exec_depend>joint_state_broadcaster</exec_depend>

4
src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp
View File

@ -311,9 +311,9 @@ std::vector<hardware_interface::CommandInterface> RobotiqGripperHardwareInterfac
gripper_force_ = gripper_force_multiplier;
command_interfaces.emplace_back(
hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_velocity", &gripper_speed_));
hardware_interface::CommandInterface(info_.joints[0].name, "max_velocity", &gripper_speed_));
command_interfaces.emplace_back(
hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_effort", &gripper_force_));
hardware_interface::CommandInterface(info_.joints[0].name, "max_effort", &gripper_force_));
command_interfaces.emplace_back(
hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_cmd", &reactivate_gripper_cmd_));
command_interfaces.emplace_back(