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

Forward controller compiled

Browse Source
This commit is contained in:
Niko Feith 2024-04-16 13:24:02 +02:00
parent 91593c5525
commit 95cf345de4
15 changed files with 521 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt Normal file
View File

@ -0,0 +1,49 @@
cmake_minimum_required(VERSION 3.8)
project(robotiq_2f_controllers)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(rclcpp REQUIRED)
find_package(controller_interface REQUIRED)
find_package(hardware_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(robotiq_2f_msgs REQUIRED)
find_package(realtime_tools REQUIRED)
include_directories(include)
add_library(${PROJECT_NAME}
src/robotiq_forward_controller.cpp
)
ament_target_dependencies(${PROJECT_NAME}
rclcpp
controller_interface
hardware_interface
pluginlib
robotiq_2f_msgs
realtime_tools
)
pluginlib_export_plugin_description_file(controller_interface controller_plugin.xml)
# Install targets
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
# Install header files
install(DIRECTORY include/
DESTINATION include
)
# Install plugin description file
install(FILES
controller_plugin.xml
DESTINATION share/${PROJECT_NAME}
)
ament_package()

7
src/robotiq_2f/robotiq_2f_controllers/controller_plugin.xml Normal file
View File

@ -0,0 +1,7 @@
<library path="robotiq_2f_controllers">
<class name="robotiq_2f_controllers/RobotiqForwardController" type="robotiq_2f_controllers::RobotiqForwardController" base_class_type="controller_interface::ControllerInterface">
<description>
Forward controller for Robotiq 2F Gripper.
</description>
</class>
</library>

99
src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_forward_controller.hpp Normal file
View File

@ -0,0 +1,99 @@
#ifndef ROBOTIQ_FORWARD_CONTROLLER_HPP
#define ROBOTIQ_FORWARD_CONTROLLER_HPP
#include <stddef.h>
#include <memory>
#include <string>
#include <vector>
#include <rclcpp/rclcpp.hpp>
#include "rclcpp/qos.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "controller_interface/controller_interface.hpp"
#include "hardware_interface/loaned_command_interface.hpp"
#include <hardware_interface/types/hardware_interface_type_values.hpp>
#include "realtime_tools/realtime_buffer.h"
#include "robotiq_2f_msgs/msg/forward_command.hpp"
namespace robotiq_2f_controllers
{
class RobotiqForwardController : public controller_interface::ControllerInterface
{
public:
CONTROLLER_INTERFACE_PUBLIC
RobotiqForwardController();
CONTROLLER_INTERFACE_PUBLIC
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::return_type update(
const rclcpp::Time & time, const rclcpp::Duration & period) override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_init() override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state) override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state) override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state) override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_cleanup(
const rclcpp_lifecycle::State & previous_state) override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_error(
const rclcpp_lifecycle::State & previous_state) override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_shutdown(
const rclcpp_lifecycle::State & previous_state) override;
protected:
std::string joint_name_;
std::vector<std::string> command_interface_types_;
// communication interface
bool new_msg_ = false;
rclcpp::Subscription<robotiq_2f_msgs::msg::ForwardCommand>::SharedPtr command_subscriber_;
std::shared_ptr<robotiq_2f_msgs::msg::ForwardCommand> command_msg_;
realtime_tools::RealtimeBuffer<std::shared_ptr<robotiq_2f_msgs::msg::ForwardCommand>>
msg_external_ptr_;
// Command Interfaces
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_position_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
max_velocity_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
max_effort_command_interface_;
std::unordered_map<std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> *>
command_interface_map_ = {
{"position", &joint_position_command_interface_},
{"max_velocity", &max_velocity_command_interface_},
{"max_effort", &max_effort_command_interface_}};
double desired_position_;
double desired_max_velocity_;
double desired_max_effort_;
};
} // end namespace robotiq_2f_controllers
#endif

47
src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_forward_controller_old.hpp Normal file
View File

@ -0,0 +1,47 @@
#ifndef ROBOTIQ_FORWARD_CONTROLLER_HPP
#define ROBOTIQ_FORWARD_CONTROLLER_HPP
#include <vector>
#include <string>
#include "controller_interface/controller_interface.hpp"
#include <rclcpp/rclcpp.hpp>
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "hardware_interface/loaned_command_interface.hpp"
#include <hardware_interface/types/hardware_interface_type_values.hpp>
#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<robotiq_2f_msgs::msg::ForwardCommand>::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

0
src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_gripper_command_controller.hpp Normal file
View File

25
src/robotiq_2f/robotiq_2f_controllers/package.xml Normal file
View File

@ -0,0 +1,25 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>robotiq_2f_controllers</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="nikolaus.feith@unileoben.ac.at">niko</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>controller_interface</depend>
<depend>hardware_interface</depend>
<depend>pluginlib</depend>
<depend>robotiq_2f_msgs</depend>
<depend>realtime_tools</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

103
src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp Normal file
View File

@ -0,0 +1,103 @@
#include "robotiq_2f_controllers/robotiq_forward_controller.hpp"
using config_type = controller_interface::interface_configuration_type;
namespace robotiq_2f_controllers
{
RobotiqForwardController::RobotiqForwardController() : controller_interface::ControllerInterface() {}
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_);
return CallbackReturn::SUCCESS;
}
controller_interface::InterfaceConfiguration RobotiqForwardController::command_interface_configuration()
const
{
controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
conf.names.reserve(command_interface_types_.size());
for (const auto & interface_type : command_interface_types_)
{
conf.names.push_back(joint_name_ + "/" + interface_type);
}
return conf;
}
controller_interface::CallbackReturn RobotiqForwardController::on_configure(const rclcpp_lifecycle::State &)
{
auto callback =
[this](const std::shared_ptr<robotiq_2f_msgs::msg::ForwardCommand> cmd_msg) -> void
{
msg_external_ptr_.writeFromNonRT(cmd_msg);
new_msg_ = true;
};
command_subscriber_ = get_node()->create_subscription<robotiq_2f_msgs::msg::ForwardCommand>(
"~/command", rclcpp::SystemDefaultsQoS(), callback);
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
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();
// 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*/)
{
if (new_msg_)
{
command_msg_ = *msg_external_ptr_.readFromRT();
desired_position_ = command_msg_->position;
desired_max_velocity_ = command_msg_->max_velocity;
desired_max_effort_ = command_msg_->max_effort;
}
// Set the hardware interface commands
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_);
return controller_interface::return_type::OK;
}
controller_interface::CallbackReturn RobotiqForwardController::on_deactivate(const rclcpp_lifecycle::State &)
{
release_interfaces();
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn RobotiqForwardController::on_cleanup(const rclcpp_lifecycle::State &)
{
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn RobotiqForwardController::on_error(const rclcpp_lifecycle::State &)
{
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn RobotiqForwardController::on_shutdown(const rclcpp_lifecycle::State &)
{
return CallbackReturn::SUCCESS;
}
}

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

@ -0,0 +1,82 @@
#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)

0
src/robotiq_2f/robotiq_2f_controllers/src/robotiq_gripper_command_controller.cpp Normal file
View File

22
src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro
View File

@ -19,7 +19,8 @@
<!-- Plugins -->
<hardware>
<xacro:if value="${use_fake_hardware}">
<plugin>mock_components/GenericSystem</plugin>
<plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
<param name="use_fake_hardware">${use_fake_hardware}</param>
<param name="mock_sensor_commands">${fake_sensor_commands}</param>
<param name="state_following_offset">0.0</param>
</xacro:if>
@ -29,15 +30,15 @@
<param name="robot_ip">${robot_ip}</param>
<param name="robot_port">${robot_port}</param>
<param name="use_fake_hardware">${use_fake_hardware}</param>
<param name="gripper_speed_multiplier">1.0</param>
<param name="gripper_force_multiplier">0.5</param>
<param name="min_position">${min_position}</param>
<param name="max_position">${max_position}</param>
<param name="min_speed">${min_speed}</param>
<param name="max_speed">${max_speed}</param>
<param name="min_force">${min_force}</param>
<param name="max_force">${max_force}</param>
</xacro:unless>
<param name="gripper_speed_multiplier">1.0</param>
<param name="gripper_force_multiplier">0.5</param>
<param name="min_position">${min_position}</param>
<param name="max_position">${max_position}</param>
<param name="min_speed">${min_speed}</param>
<param name="max_speed">${max_speed}</param>
<param name="min_force">${min_force}</param>
<param name="max_force">${max_force}</param>
</hardware>
<!-- Joint interfaces -->
@ -50,7 +51,8 @@
<state_interface name="velocity"/>
</joint>
<!-- When simulating we need to include the rest of the gripper joints -->
<xacro:if value="${use_fake_hardware}">
<!-- <xacro:if value="${use_fake_hardware}"> -->
<xacro:if value="false">
<joint name="${prefix}right_outer_knuckle_joint">
<param name="mimic">${prefix}finger_joint</param>
<param name="multiplier">-1</param>

22
src/robotiq_2f/robotiq_2f_msgs/CMakeLists.txt Normal file
View File

@ -0,0 +1,22 @@
cmake_minimum_required(VERSION 3.8)
project(robotiq_2f_msgs)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(builtin_interfaces REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/ForwardCommand.msg"
# Add more messages here
DEPENDENCIES builtin_interfaces
)
ament_export_dependencies(rosidl_default_runtime)
ament_package()

3
src/robotiq_2f/robotiq_2f_msgs/msg/ForwardCommand.msg Normal file
View File

@ -0,0 +1,3 @@
float64 position
float64 max_velocity
float64 max_effort

25
src/robotiq_2f/robotiq_2f_msgs/package.xml Normal file
View File

@ -0,0 +1,25 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>robotiq_2f_msgs</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="nikolaus.feith@unileoben.ac.at">niko</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>std_msgs</depend>
<depend>builtin_interfaces</depend>
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

11
src/robotiq_2f/robotiq_controllers/src/robotiq_activation_controller.cpp
View File

@ -105,17 +105,12 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Roboti
bool RobotiqActivationController::reactivateGripper(std_srvs::srv::Trigger::Request::SharedPtr /*req*/,
std_srvs::srv::Trigger::Response::SharedPtr resp)
{
RCLCPP_INFO(get_node()->get_logger(), "Received reactivation request.");
command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].set_value(ASYNC_WAITING);
command_interfaces_[REACTIVATE_GRIPPER_CMD].set_value(1.0);
RCLCPP_INFO(get_node()->get_logger(), "Reactivation command sent to hardware interface.");
int attempts = 0;
int max_attempts = 1000;
while (command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value() == ASYNC_WAITING && attempts < max_attempts) {
std::this_thread::sleep_for(std::chrono::milliseconds(50));
attempts++;
RCLCPP_INFO(get_node()->get_logger(), "Attempt %d: Checking response from hardware interface: %f", attempts, command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value());
while (command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value() == ASYNC_WAITING)
{
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
resp->success = command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value();

44
src/ur_robotiq_description/config/ur_robotiq_calibration.yaml Normal file
View File

@ -0,0 +1,44 @@
kinematics:
shoulder:
x: 0
y: 0
z: 0.1519161231793138
roll: -0
pitch: 0
yaw: -8.3688888041777445e-08
upper_arm:
x: -9.8255319984929057e-05
y: 0
z: 0
roll: 1.5713473529266058
pitch: 0
yaw: 4.5372573280303068e-06
forearm:
x: -0.24361145101605494
y: 0
z: 0
roll: 3.138959939545904
pitch: 3.141417818890341
yaw: 3.1415861917625292
wrist_1:
x: -0.21328351386382516
y: -0.00058695912107010507
z: 0.13086135281159214
roll: 0.0044853210843803608
pitch: 0.00071590512460444685
yaw: -2.0286176909971606e-06
wrist_2:
x: 9.0086536104922084e-05
y: -0.085314349728116412
z: -6.1195228207677147e-05
roll: 1.5715136178239859
pitch: 0
yaw: 1.3915445690113049e-06
wrist_3:
x: -0.00014076969524819022
y: 0.092309359181826575
z: -0.00011628039579221433
roll: 1.5695366459205147
pitch: 3.1415926535897931
yaw: 3.1415925648453848
hash: calib_2535041911463403862