debugging
This commit is contained in:
parent
625c5082b0
commit
719502e631
@ -7,14 +7,6 @@ repositories:
|
|||||||
type: git
|
type: git
|
||||||
url: https://github.com/NikoFeith/Universal_Robots_ROS2_Driver.git
|
url: https://github.com/NikoFeith/Universal_Robots_ROS2_Driver.git
|
||||||
version: humble
|
version: humble
|
||||||
serial:
|
|
||||||
type: git
|
|
||||||
url: https://github.com/tylerjw/serial.git
|
|
||||||
version: ros2
|
|
||||||
ros2_robotiq_gripper:
|
|
||||||
type: git
|
|
||||||
url: https://github.com/NikoFeith/ros2_robotiq_gripper.git
|
|
||||||
version: main
|
|
||||||
cartesian_controllers:
|
cartesian_controllers:
|
||||||
type: git
|
type: git
|
||||||
url: https://github.com/NikoFeith/cartesian_controllers.git
|
url: https://github.com/NikoFeith/cartesian_controllers.git
|
||||||
|
|||||||
@ -1,26 +0,0 @@
|
|||||||
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(ament_cmake REQUIRED)
|
|
||||||
# uncomment the following section in order to fill in
|
|
||||||
# further dependencies manually.
|
|
||||||
# find_package(<dependency> REQUIRED)
|
|
||||||
|
|
||||||
if(BUILD_TESTING)
|
|
||||||
find_package(ament_lint_auto REQUIRED)
|
|
||||||
# the following line skips the linter which checks for copyrights
|
|
||||||
# comment the line when a copyright and license is added to all source files
|
|
||||||
set(ament_cmake_copyright_FOUND TRUE)
|
|
||||||
# the following line skips cpplint (only works in a git repo)
|
|
||||||
# comment the line when this package is in a git repo and when
|
|
||||||
# a copyright and license is added to all source files
|
|
||||||
set(ament_cmake_cpplint_FOUND TRUE)
|
|
||||||
ament_lint_auto_find_test_dependencies()
|
|
||||||
endif()
|
|
||||||
|
|
||||||
ament_package()
|
|
||||||
@ -1,18 +0,0 @@
|
|||||||
<?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>
|
|
||||||
|
|
||||||
<test_depend>ament_lint_auto</test_depend>
|
|
||||||
<test_depend>ament_lint_common</test_depend>
|
|
||||||
|
|
||||||
<export>
|
|
||||||
<build_type>ament_cmake</build_type>
|
|
||||||
</export>
|
|
||||||
</package>
|
|
||||||
@ -19,7 +19,7 @@
|
|||||||
</xacro:if>
|
</xacro:if>
|
||||||
<xacro:unless value="${use_fake_hardware}">
|
<xacro:unless value="${use_fake_hardware}">
|
||||||
<plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
|
<plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
|
||||||
<param name="gripper_closed_position">0.14</param>
|
<param name="gripper_closed_position">0.695</param>
|
||||||
<param name="robot_ip">${robot_ip}</param>
|
<param name="robot_ip">${robot_ip}</param>
|
||||||
<param name="robot_port">${robot_port}</param>
|
<param name="robot_port">${robot_port}</param>
|
||||||
<param name="gripper_speed_multiplier">1.0</param>
|
<param name="gripper_speed_multiplier">1.0</param>
|
||||||
@ -32,7 +32,7 @@
|
|||||||
<joint name="${prefix}finger_joint">
|
<joint name="${prefix}finger_joint">
|
||||||
<command_interface name="position" />
|
<command_interface name="position" />
|
||||||
<state_interface name="position">
|
<state_interface name="position">
|
||||||
<param name="initial_value">0.14</param>
|
<param name="initial_value">0.695</param>
|
||||||
</state_interface>
|
</state_interface>
|
||||||
<state_interface name="velocity"/>
|
<state_interface name="velocity"/>
|
||||||
</joint>
|
</joint>
|
||||||
|
|||||||
@ -0,0 +1,13 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
|
||||||
|
<!-- parameters -->
|
||||||
|
<xacro:arg name="use_fake_hardware" default="false" />
|
||||||
|
|
||||||
|
<!-- Import macros -->
|
||||||
|
<xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_2f_140_macro.urdf.xacro" />
|
||||||
|
|
||||||
|
<link name="world" />
|
||||||
|
<xacro:robotiq_gripper name="RobotiqGripperHardwareInterface" prefix="" parent="world" use_fake_hardware="$(arg use_fake_hardware)">
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
</xacro:robotiq_gripper>
|
||||||
|
</robot>
|
||||||
@ -0,0 +1,421 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
|
||||||
|
<xacro:macro name="robotiq_gripper" params="
|
||||||
|
name
|
||||||
|
prefix
|
||||||
|
parent
|
||||||
|
*origin
|
||||||
|
include_ros2_control:=true
|
||||||
|
use_fake_hardware:=false
|
||||||
|
fake_sensor_commands:=false
|
||||||
|
robot_ip:=192.168.1.1
|
||||||
|
robot_port:=63352>
|
||||||
|
|
||||||
|
<!-- ros2 control include -->
|
||||||
|
<xacro:include filename="$(find robotiq_2f_description)/urdf/2f_140.ros2_control.xacro" />
|
||||||
|
<!-- if we are simulating or directly communicating with the gripper we need a ros2 control instance -->
|
||||||
|
<xacro:if value="${include_ros2_control}">
|
||||||
|
<xacro:robotiq_gripper_ros2_control
|
||||||
|
name="${name}" prefix="${prefix}"
|
||||||
|
use_fake_hardware="${use_fake_hardware}"
|
||||||
|
fake_sensor_commands="${fake_sensor_commands}"
|
||||||
|
robot_ip="${robot_ip}"
|
||||||
|
robot_port="${robot_port}"/>
|
||||||
|
</xacro:if>
|
||||||
|
|
||||||
|
<!-- this is a temporary link to rotate the 2f-140 gripper to match the 2f-85 -->
|
||||||
|
<link name="${prefix}robotiq_base_link"/>
|
||||||
|
<joint name="${prefix}robotiq_base_joint" type="fixed">
|
||||||
|
<parent link="${parent}" />
|
||||||
|
<child link="${prefix}robotiq_base_link" />
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 ${pi/2}" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_base_link">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="8.625E-08 -4.6583E-06 0.03145" rpy="0 0 0" />
|
||||||
|
<mass value="0.22652" />
|
||||||
|
<inertia ixx="0.00020005" ixy="-4.2442E-10" ixz="-2.9069E-10" iyy="0.00017832" iyz="-3.4402E-08" izz="0.00013478" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_base_link.stl" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.1 0.1 0.1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_base_link.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_left_outer_knuckle">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331" rpy="0 0 0" />
|
||||||
|
<mass value="0.00853198276973456" />
|
||||||
|
<inertia
|
||||||
|
ixx="2.89328108496468E-06"
|
||||||
|
ixy="-1.57935047237397E-19"
|
||||||
|
ixz="-1.93980378593255E-19"
|
||||||
|
iyy="1.86719750325683E-06"
|
||||||
|
iyz="-1.21858577871576E-06"
|
||||||
|
izz="1.21905238907251E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_right_outer_knuckle">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331" rpy="0 0 0" />
|
||||||
|
<mass value="0.00853198276973456" />
|
||||||
|
<inertia
|
||||||
|
ixx="2.89328108496468E-06"
|
||||||
|
ixy="-1.57935047237397E-19"
|
||||||
|
ixz="-1.93980378593255E-19"
|
||||||
|
iyy="1.86719750325683E-06"
|
||||||
|
iyz="-1.21858577871576E-06"
|
||||||
|
izz="1.21905238907251E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_left_outer_finger">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385" rpy="0 0 0" />
|
||||||
|
<mass value="0.022614240507152" />
|
||||||
|
<inertia
|
||||||
|
ixx="1.52518312458174E-05"
|
||||||
|
ixy="9.76583423954399E-10"
|
||||||
|
ixz="-5.43838577022588E-10"
|
||||||
|
iyy="6.17694243867776E-06"
|
||||||
|
iyz="6.78636130740228E-06"
|
||||||
|
izz="1.16494917907219E-05" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.1 0.1 0.1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_right_outer_finger">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385" rpy="0 0 0" />
|
||||||
|
<mass value="0.022614240507152" />
|
||||||
|
<inertia
|
||||||
|
ixx="1.52518312458174E-05"
|
||||||
|
ixy="9.76583423954399E-10"
|
||||||
|
ixz="-5.43838577022588E-10"
|
||||||
|
iyy="6.17694243867776E-06"
|
||||||
|
iyz="6.78636130740228E-06"
|
||||||
|
izz="1.16494917907219E-05" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.1 0.1 0.1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_left_inner_knuckle">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166" rpy="0 0 0" />
|
||||||
|
<mass value="0.0271177346495152" />
|
||||||
|
<inertia
|
||||||
|
ixx="2.61910379223783E-05"
|
||||||
|
ixy="-2.43616858946494E-07"
|
||||||
|
ixz="-6.37789906117123E-09"
|
||||||
|
iyy="2.8270243746167E-06"
|
||||||
|
iyz="-5.37200748039765E-07"
|
||||||
|
izz="2.83695868220296E-05" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.1 0.1 0.1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_right_inner_knuckle">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166" rpy="0 0 0" />
|
||||||
|
<mass value="0.0271177346495152" />
|
||||||
|
<inertia
|
||||||
|
ixx="2.61910379223783E-05"
|
||||||
|
ixy="-2.43616858946494E-07"
|
||||||
|
ixz="-6.37789906117123E-09"
|
||||||
|
iyy="2.8270243746167E-06"
|
||||||
|
iyz="-5.37200748039765E-07"
|
||||||
|
izz="2.83695868220296E-05" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.1 0.1 0.1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_left_inner_finger">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257" rpy="0 0 0" />
|
||||||
|
<mass value="0.0104003125914103" />
|
||||||
|
<inertia
|
||||||
|
ixx="2.71909453810972E-06"
|
||||||
|
ixy="1.35402465472579E-21"
|
||||||
|
ixz="-7.1817349065269E-22"
|
||||||
|
iyy="7.69100314106116E-07"
|
||||||
|
iyz="6.74715432769696E-07"
|
||||||
|
izz="2.30315190420171E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.1 0.1 0.1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_right_inner_finger">
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257" rpy="0 0 0" />
|
||||||
|
<mass value="0.0104003125914103" />
|
||||||
|
<inertia
|
||||||
|
ixx="2.71909453810972E-06"
|
||||||
|
ixy="1.35402465472579E-21"
|
||||||
|
ixz="-7.1817349065269E-22"
|
||||||
|
iyy="7.69100314106116E-07"
|
||||||
|
iyz="6.74715432769696E-07"
|
||||||
|
izz="2.30315190420171E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.1 0.1 0.1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_left_inner_finger_pad">
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<box size="0.027 0.065 0.0075"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.9 0.9 0.9 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<box size="0.03 0.07 0.0075"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.9 0.0 0.0 1" />
|
||||||
|
</material>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_140_right_inner_finger_pad">
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<box size="0.027 0.065 0.0075"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.9 0.9 0.9 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<box size="0.03 0.07 0.0075"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.9 0.0 0.0 1" />
|
||||||
|
</material>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
<!-- Joints-->
|
||||||
|
<joint name="${prefix}robotiq_140_base_joint" type="fixed">
|
||||||
|
<parent link="${prefix}robotiq_base_link" />
|
||||||
|
<child link="${prefix}robotiq_140_base_link" />
|
||||||
|
<xacro:insert_block name="origin" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}finger_joint" type="revolute">
|
||||||
|
<origin xyz="0 -0.030601 0.054905" rpy="${pi / 2 + .725} 0 0" />
|
||||||
|
<parent link="${prefix}robotiq_140_base_link" />
|
||||||
|
<child link="${prefix}robotiq_140_left_outer_knuckle" />
|
||||||
|
<axis xyz="-1 0 0" />
|
||||||
|
<limit lower="0" upper="0.7" velocity="2.0" effort="1000" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}right_outer_knuckle_joint" type="revolute">
|
||||||
|
<origin xyz="0 0.030601 0.054905" rpy="${pi / 2 + .725} 0 ${pi}" />
|
||||||
|
<parent link="${prefix}robotiq_140_base_link" />
|
||||||
|
<child link="${prefix}robotiq_140_right_outer_knuckle" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
<limit lower="-0.725" upper="0.725" velocity="2.0" effort="1000" />
|
||||||
|
<mimic joint="${prefix}finger_joint" multiplier="-1" offset="0" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}left_outer_finger_joint" type="fixed">
|
||||||
|
<origin xyz="0 0.01821998610742 0.0260018192872234" rpy="0 0 0" />
|
||||||
|
<parent link="${prefix}robotiq_140_left_outer_knuckle" />
|
||||||
|
<child link="${prefix}robotiq_140_left_outer_finger" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}right_outer_finger_joint" type="fixed">
|
||||||
|
<origin xyz="0 0.01821998610742 0.0260018192872234" rpy="0 0 0" />
|
||||||
|
<parent link="${prefix}robotiq_140_right_outer_knuckle" />
|
||||||
|
<child link="${prefix}robotiq_140_right_outer_finger" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}left_inner_knuckle_joint" type="revolute">
|
||||||
|
<origin xyz="0 -0.0127 0.06142" rpy="${pi / 2 + .725} 0 0" />
|
||||||
|
<parent link="${prefix}robotiq_140_base_link" />
|
||||||
|
<child link="${prefix}robotiq_140_left_inner_knuckle" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
<limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
|
||||||
|
<mimic joint="${prefix}finger_joint" multiplier="-1" offset="0" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}right_inner_knuckle_joint" type="revolute">
|
||||||
|
<origin xyz="0 0.0127 0.06142" rpy="${pi / 2 + .725} 0 ${-pi}" />
|
||||||
|
<parent link="${prefix}robotiq_140_base_link" />
|
||||||
|
<child link="${prefix}robotiq_140_right_inner_knuckle" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
<limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
|
||||||
|
<mimic joint="${prefix}finger_joint" multiplier="-1" offset="0" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}left_inner_finger_joint" type="revolute">
|
||||||
|
<origin xyz="0 0.0817554015893473 -0.0282203446692936" rpy="-0.725 0 0" />
|
||||||
|
<parent link="${prefix}robotiq_140_left_outer_finger" />
|
||||||
|
<child link="${prefix}robotiq_140_left_inner_finger" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
<limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
|
||||||
|
<mimic joint="${prefix}finger_joint" multiplier="1" offset="0" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}right_inner_finger_joint" type="revolute">
|
||||||
|
<origin xyz="0 0.0817554015893473 -0.0282203446692936" rpy="-0.725 0 0" />
|
||||||
|
<parent link="${prefix}robotiq_140_right_outer_finger" />
|
||||||
|
<child link="${prefix}robotiq_140_right_inner_finger" />
|
||||||
|
<axis xyz="1 0 0" />
|
||||||
|
<limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
|
||||||
|
<mimic joint="${prefix}finger_joint" multiplier="1" offset="0" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}left_inner_finger_pad_joint" type="fixed">
|
||||||
|
<origin xyz="0 0.0457554015893473 -0.0272203446692936" rpy="0 0 0" />
|
||||||
|
<parent link="${prefix}robotiq_140_left_inner_finger" />
|
||||||
|
<child link="${prefix}robotiq_140_left_inner_finger_pad" />
|
||||||
|
<axis xyz="0 0 1" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}right_inner_finger_pad_joint" type="fixed">
|
||||||
|
<origin xyz="0 0.0457554015893473 -0.0272203446692936" rpy="0 0 0" />
|
||||||
|
<parent link="${prefix}robotiq_140_right_inner_finger" />
|
||||||
|
<child link="${prefix}robotiq_140_right_inner_finger_pad" />
|
||||||
|
<axis xyz="0 0 1" />
|
||||||
|
</joint>
|
||||||
|
</xacro:macro>
|
||||||
|
</robot>
|
||||||
@ -0,0 +1,87 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||||
|
|
||||||
|
<xacro:macro name="robotiq_gripper_ros2_control" params="
|
||||||
|
name
|
||||||
|
prefix
|
||||||
|
use_fake_hardware:=false
|
||||||
|
fake_sensor_commands:=false
|
||||||
|
robot_ip:=192.168.1.1
|
||||||
|
robot_port:=63352>
|
||||||
|
|
||||||
|
<ros2_control name="${name}" type="system">
|
||||||
|
<!-- Plugins -->
|
||||||
|
<hardware>
|
||||||
|
<xacro:if value="${use_fake_hardware}">
|
||||||
|
<plugin>mock_components/GenericSystem</plugin>
|
||||||
|
<param name="fake_sensor_commands">${fake_sensor_commands}</param>
|
||||||
|
<param name="state_following_offset">0.0</param>
|
||||||
|
</xacro:if>
|
||||||
|
<xacro:unless value="${use_fake_hardware}">
|
||||||
|
<plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
|
||||||
|
<param name="gripper_closed_position">0.7929</param>
|
||||||
|
<param name="robot_ip">${robot_ip}</param>
|
||||||
|
<param name="robot_port">${robot_port}</param>
|
||||||
|
<param name="gripper_speed_multiplier">1.0</param>
|
||||||
|
<param name="gripper_force_multiplier">0.5</param>
|
||||||
|
</xacro:unless>
|
||||||
|
</hardware>
|
||||||
|
|
||||||
|
<!-- Joint interfaces -->
|
||||||
|
<!-- With Hardware, they handle mimic joints, so we only need this command interface activated -->
|
||||||
|
<joint name="${prefix}finger_joint">
|
||||||
|
<command_interface name="position" />
|
||||||
|
<state_interface name="position">
|
||||||
|
<param name="initial_value">0.7929</param>
|
||||||
|
</state_interface>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<!-- When simulating we need to include the rest of the gripper joints -->
|
||||||
|
<xacro:if value="${use_fake_hardware}">
|
||||||
|
<joint name="${prefix}left_inner_knuckle_joint">
|
||||||
|
<param name="mimic">${prefix}finger_joint</param>
|
||||||
|
<param name="multiplier">-1</param>
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="${prefix}left_inner_finger_joint">
|
||||||
|
<param name="mimic">${prefix}finger_joint</param>
|
||||||
|
<param name="multiplier">1</param>
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="${prefix}right_outer_knuckle_joint">
|
||||||
|
<param name="mimic">${prefix}finger_joint</param>
|
||||||
|
<param name="multiplier">-1</param>
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="${prefix}robotiq_140_left_finger_tip_joint">
|
||||||
|
<param name="mimic">${prefix}robotiq_140_left_knuckle_joint</param>
|
||||||
|
<param name="multiplier">-1</param>
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
<joint name="${prefix}robotiq_140_right_finger_tip_joint">
|
||||||
|
<param name="mimic">${prefix}robotiq_140_left_knuckle_joint</param>
|
||||||
|
<param name="multiplier">1</param>
|
||||||
|
<command_interface name="position"/>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
</joint>
|
||||||
|
</xacro:if>
|
||||||
|
|
||||||
|
<!-- Only add this with fake hardware mode -->
|
||||||
|
<gpio name="reactivate_gripper">
|
||||||
|
<command_interface name="reactivate_gripper_cmd" />
|
||||||
|
<command_interface name="reactivate_gripper_response" />
|
||||||
|
</gpio>
|
||||||
|
|
||||||
|
</ros2_control>
|
||||||
|
</xacro:macro>
|
||||||
|
|
||||||
|
</robot>
|
||||||
@ -0,0 +1,13 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
|
||||||
|
<!-- parameters -->
|
||||||
|
<xacro:arg name="use_fake_hardware" default="true" />
|
||||||
|
|
||||||
|
<!-- Import macros -->
|
||||||
|
<xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_2f_85_macro.urdf.xacro" />
|
||||||
|
|
||||||
|
<link name="world" />
|
||||||
|
<xacro:robotiq_gripper name="RobotiqGripperHardwareInterface" prefix="" parent="world" use_fake_hardware="$(arg use_fake_hardware)">
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
</xacro:robotiq_gripper>
|
||||||
|
</robot>
|
||||||
@ -0,0 +1,291 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
|
||||||
|
<xacro:macro name="robotiq_gripper" params="
|
||||||
|
name
|
||||||
|
prefix
|
||||||
|
parent
|
||||||
|
*origin
|
||||||
|
include_ros2_control:=true
|
||||||
|
use_fake_hardware:=false
|
||||||
|
fake_sensor_commands:=false
|
||||||
|
robot_ip:=192.168.1.1
|
||||||
|
robot_port:=63352>
|
||||||
|
|
||||||
|
<!-- ros2 control include -->
|
||||||
|
<xacro:include filename="$(find robotiq_2f_description)/urdf/2f_85.ros2_control.xacro" />
|
||||||
|
<!-- if we are simulating or directly communicating with the gripper we need a ros2 control instance -->
|
||||||
|
<xacro:if value="${include_ros2_control}">
|
||||||
|
<xacro:robotiq_gripper_ros2_control
|
||||||
|
name="${name}" prefix="${prefix}"
|
||||||
|
use_fake_hardware="${use_fake_hardware}"
|
||||||
|
fake_sensor_commands="${fake_sensor_commands}"
|
||||||
|
robot_ip="${robot_ip}"/>
|
||||||
|
robot_port="${robot_port}"/>
|
||||||
|
</xacro:if>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_85_base_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/robotiq_base.dae" />
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/robotiq_base.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.0 2.274e-05 0.03232288" rpy="0 0 0" />
|
||||||
|
<mass value="6.6320197e-01" />
|
||||||
|
<inertia ixx="5.1617816e-04" iyy="5.8802208e-04" izz="3.9462776e-04" ixy="2.936e-8" ixz="0.0" iyz="-3.2296e-7" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_85_left_knuckle_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/left_knuckle.dae" />
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/left_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.01213197 0.0002 -0.00058647" rpy="0 0 0" />
|
||||||
|
<mass value="1.384773208e-02" />
|
||||||
|
<inertia ixx="3.5232e-7" iyy="2.31944e-6" izz="2.23136e-6" ixy="0.0" ixz="1.1744e-7" iyz="0" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_85_right_knuckle_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/right_knuckle.dae" />
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/right_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="-0.01213197 0.0002 -0.00058647" rpy="0 0 0" />
|
||||||
|
<mass value="1.384773208e-02" />
|
||||||
|
<inertia ixx="3.5232e-7" iyy="2.31944e-6" izz="2.23136e-6" ixy="0.0" ixz="-1.1744e-7" iyz="0.0" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_85_left_finger_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/left_finger.dae" />
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/left_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.00346899 -0.00079447 0.01867121" rpy="0 0 0" />
|
||||||
|
<mass value="4.260376752e-02" />
|
||||||
|
<inertia ixx="1.385792000000000e-05" iyy="1.183208e-05" izz="5.19672e-06" ixy="0.0" ixz="-2.17264e-06" iyz="0.0" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_85_right_finger_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/right_finger.dae" />
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/right_finger.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="-0.00346899 -5.53e-06 0.01867121" rpy="0 0 0" />
|
||||||
|
<mass value="4.260376752000000e-02" />
|
||||||
|
<inertia ixx="1.385792e-05" iyy="1.183208e-05" izz="5.19672e-06" ixy="0.0" ixz="2.17264e-06" iyz="0.0" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_85_left_inner_knuckle_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/left_inner_knuckle.dae" />
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/left_inner_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.01897699 0.00015001 0.02247101" rpy="0 0 0" />
|
||||||
|
<mass value="2.969376448e-02" />
|
||||||
|
<inertia ixx="9.57136e-06" iyy="8.69056e-06" izz="8.19144e-06" ixy="0.0" ixz="-3.93424e-06" iyz="0.0" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_85_right_inner_knuckle_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/right_inner_knuckle.dae" />
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/right_inner_knuckle.stl" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="-0.01926824 5.001e-05 0.02222178" rpy="0 0 0" />
|
||||||
|
<mass value="2.969376448e-02" />
|
||||||
|
<inertia ixx="9.42456e-06" iyy="8.69056e-06" izz="8.33824e-06" ixy="0.0" ixz="3.9636e-06" iyz="0.0" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_85_left_finger_tip_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/left_finger_tip.dae" />
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/left_finger_tip.stl" />
|
||||||
|
</geometry>
|
||||||
|
<surface>
|
||||||
|
<friction>
|
||||||
|
<ode>
|
||||||
|
<mu1>100000.0</mu1>
|
||||||
|
<mu2>100000.0</mu2>
|
||||||
|
</ode>
|
||||||
|
</friction>
|
||||||
|
<contact>
|
||||||
|
<ode>
|
||||||
|
<kp>1e+5</kp>
|
||||||
|
<kd>1</kd>
|
||||||
|
<soft_cfm>0</soft_cfm>
|
||||||
|
<soft_erp>0.2</soft_erp>
|
||||||
|
<minDepth>0.002</minDepth>
|
||||||
|
<maxVel>0</maxVel>
|
||||||
|
</ode>
|
||||||
|
</contact>
|
||||||
|
</surface>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="-0.01456706 -0.0008 0.01649701" rpy="0 0 0" />
|
||||||
|
<mass value="4.268588744e-02" />
|
||||||
|
<inertia ixx="1.048152e-05" iyy="1.197888e-05" izz="4.22784e-06" ixy="0.0" ixz="3.5232e-6" iyz="0.0" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name="${prefix}robotiq_85_right_finger_tip_link">
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/right_finger_tip.dae" />
|
||||||
|
</geometry>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/right_finger_tip.stl" />
|
||||||
|
</geometry>
|
||||||
|
<surface>
|
||||||
|
<friction>
|
||||||
|
<ode>
|
||||||
|
<mu1>100000.0</mu1>
|
||||||
|
<mu2>100000.0</mu2>
|
||||||
|
</ode>
|
||||||
|
</friction>
|
||||||
|
<contact>
|
||||||
|
<ode>
|
||||||
|
<kp>1e+5</kp>
|
||||||
|
<kd>1</kd>
|
||||||
|
<soft_cfm>0</soft_cfm>
|
||||||
|
<soft_erp>0.2</soft_erp>
|
||||||
|
<minDepth>0.002</minDepth>
|
||||||
|
<maxVel>0</maxVel>
|
||||||
|
</ode>
|
||||||
|
</contact>
|
||||||
|
</surface>
|
||||||
|
</collision>
|
||||||
|
<inertial>
|
||||||
|
<origin xyz="0.01456706 5e-05 0.01649701" rpy="0 0 0" />
|
||||||
|
<mass value="4.268588744e-02" />
|
||||||
|
<inertia ixx="1.048152e-05" iyy="1.197888e-05" izz="4.22784e-06" ixy="0.0" ixz="-3.5232e-06" iyz="0.0" />
|
||||||
|
</inertial>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name="${prefix}robotiq_85_base_joint" type="fixed">
|
||||||
|
<parent link="${parent}" />
|
||||||
|
<child link="${prefix}robotiq_85_base_link" />
|
||||||
|
<xacro:insert_block name="origin" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}robotiq_85_left_knuckle_joint" type="revolute">
|
||||||
|
<parent link="${prefix}robotiq_85_base_link" />
|
||||||
|
<child link="${prefix}robotiq_85_left_knuckle_link" />
|
||||||
|
<axis xyz="0 -1 0" />
|
||||||
|
<origin xyz="0.03060114 0.0 0.05490452" rpy="0 0 0" />
|
||||||
|
<limit lower="0.0" upper="0.8" velocity="0.5" effort="50" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}robotiq_85_right_knuckle_joint" type="revolute">
|
||||||
|
<parent link="${prefix}robotiq_85_base_link" />
|
||||||
|
<child link="${prefix}robotiq_85_right_knuckle_link" />
|
||||||
|
<axis xyz="0 -1 0" />
|
||||||
|
<origin xyz="-0.03060114 0.0 0.05490452" rpy="0 0 0" />
|
||||||
|
<limit lower="-0.8" upper="0.0" velocity="0.5" effort="50" />
|
||||||
|
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" multiplier="-1" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}robotiq_85_left_finger_joint" type="fixed">
|
||||||
|
<parent link="${prefix}robotiq_85_left_knuckle_link" />
|
||||||
|
<child link="${prefix}robotiq_85_left_finger_link" />
|
||||||
|
<origin xyz="0.03152616 0.0 -0.00376347" rpy="0 0 0" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}robotiq_85_right_finger_joint" type="fixed">
|
||||||
|
<parent link="${prefix}robotiq_85_right_knuckle_link" />
|
||||||
|
<child link="${prefix}robotiq_85_right_finger_link" />
|
||||||
|
<origin xyz="-0.03152616 0.0 -0.00376347" rpy="0 0 0" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}robotiq_85_left_inner_knuckle_joint" type="continuous">
|
||||||
|
<parent link="${prefix}robotiq_85_base_link" />
|
||||||
|
<child link="${prefix}robotiq_85_left_inner_knuckle_link" />
|
||||||
|
<axis xyz="0 -1 0" />
|
||||||
|
<origin xyz="0.0127 0.0 0.06142" rpy="0 0 0" />
|
||||||
|
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}robotiq_85_right_inner_knuckle_joint" type="continuous">
|
||||||
|
<parent link="${prefix}robotiq_85_base_link" />
|
||||||
|
<child link="${prefix}robotiq_85_right_inner_knuckle_link" />
|
||||||
|
<axis xyz="0 -1 0" />
|
||||||
|
<origin xyz="-0.0127 0.0 0.06142" rpy="0 0 0" />
|
||||||
|
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" multiplier="-1" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}robotiq_85_left_finger_tip_joint" type="continuous">
|
||||||
|
<parent link="${prefix}robotiq_85_left_finger_link" />
|
||||||
|
<child link="${prefix}robotiq_85_left_finger_tip_link" />
|
||||||
|
<axis xyz="0 -1 0" />
|
||||||
|
<origin xyz="0.00563134 0.0 0.04718515" rpy="0 0 0" />
|
||||||
|
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" multiplier="-1" />
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name="${prefix}robotiq_85_right_finger_tip_joint" type="continuous">
|
||||||
|
<parent link="${prefix}robotiq_85_right_finger_link" />
|
||||||
|
<child link="${prefix}robotiq_85_right_finger_tip_link" />
|
||||||
|
<axis xyz="0 -1 0" />
|
||||||
|
<origin xyz="-0.00563134 0.0 0.04718515" rpy="0 0 0" />
|
||||||
|
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" />
|
||||||
|
</joint>
|
||||||
|
</xacro:macro>
|
||||||
|
</robot>
|
||||||
@ -1,28 +1,48 @@
|
|||||||
cmake_minimum_required(VERSION 3.8)
|
cmake_minimum_required(VERSION 3.5)
|
||||||
project(robotiq_2f_interface)
|
project(robotiq_2f_interface)
|
||||||
|
|
||||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
# Default to C++17
|
||||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
if(NOT CMAKE_CXX_STANDARD)
|
||||||
|
set(CMAKE_CXX_STANDARD 17)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
# find dependencies
|
|
||||||
find_package(ament_cmake REQUIRED)
|
find_package(ament_cmake REQUIRED)
|
||||||
find_package(rclcpp REQUIRED)
|
find_package(rclcpp REQUIRED)
|
||||||
find_package(rclcpp_lifecycle REQUIRED)
|
|
||||||
find_package(hardware_interface REQUIRED)
|
find_package(hardware_interface REQUIRED)
|
||||||
find_package(lifecycle_msgs REQUIRED)
|
|
||||||
find_package(pluginlib REQUIRED)
|
find_package(pluginlib REQUIRED)
|
||||||
|
|
||||||
if(BUILD_TESTING)
|
include_directories(include)
|
||||||
find_package(ament_lint_auto REQUIRED)
|
|
||||||
# the following line skips the linter which checks for copyrights
|
# Declare a C++ library
|
||||||
# comment the line when a copyright and license is added to all source files
|
add_library(${PROJECT_NAME} SHARED
|
||||||
set(ament_cmake_copyright_FOUND TRUE)
|
src/hardware_interface.cpp
|
||||||
# the following line skips cpplint (only works in a git repo)
|
src/Robotiq2fSocketAdapter.cpp
|
||||||
# comment the line when this package is in a git repo and when
|
)
|
||||||
# a copyright and license is added to all source files
|
|
||||||
set(ament_cmake_cpplint_FOUND TRUE)
|
ament_target_dependencies(${PROJECT_NAME}
|
||||||
ament_lint_auto_find_test_dependencies()
|
rclcpp
|
||||||
endif()
|
hardware_interface
|
||||||
|
pluginlib
|
||||||
|
)
|
||||||
|
|
||||||
|
pluginlib_export_plugin_description_file(hardware_interface hardware_interface_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
|
||||||
|
hardware_interface_plugin.xml
|
||||||
|
DESTINATION share/${PROJECT_NAME}
|
||||||
|
)
|
||||||
|
|
||||||
ament_package()
|
ament_package()
|
||||||
|
|||||||
@ -0,0 +1,9 @@
|
|||||||
|
<library path="robotiq_driver_plugin">
|
||||||
|
<class name="robotiq_2f_interface/RobotiqGripperHardwareInterface"
|
||||||
|
type="robtiq_2f_interface::RobotiqGripperHardwareInterface"
|
||||||
|
base_class_type="hardware_interface::SystemInterface">
|
||||||
|
<description>
|
||||||
|
ROS2 controller for the Robotiq gripper.
|
||||||
|
</description>
|
||||||
|
</class>
|
||||||
|
</library>
|
||||||
@ -0,0 +1,101 @@
|
|||||||
|
// MockRobotiq2fSocketAdapter.hpp
|
||||||
|
|
||||||
|
#ifndef MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||||
|
#define MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
#include <map>
|
||||||
|
#include <atomic>
|
||||||
|
#include <tuple>
|
||||||
|
|
||||||
|
namespace robotiq{
|
||||||
|
class MockRobotiq2fSocketAdapter : public SocketAdapterBase {
|
||||||
|
public:
|
||||||
|
MockRobotiq2fSocketAdapter() : isConnected(false), gripperPosition(0), gripperForce(0), gripperSpeed(0), isActive(false) {}
|
||||||
|
|
||||||
|
~MockRobotiq2fSocketAdapter() override {
|
||||||
|
if (movementThread && movementThread->joinable()) {
|
||||||
|
movementThread->join();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool connect(const std::string& hostname, int port) override {
|
||||||
|
isConnected.store(true);
|
||||||
|
return true; // Simulate successful connection
|
||||||
|
}
|
||||||
|
|
||||||
|
void disconnect() override {
|
||||||
|
isConnected.store(false);
|
||||||
|
}
|
||||||
|
|
||||||
|
int getGripperPosition() override {
|
||||||
|
return gripperPosition.load();
|
||||||
|
}
|
||||||
|
|
||||||
|
int getGripperForce() override {
|
||||||
|
// Assuming this is supposed to mimic `force()`
|
||||||
|
return gripperForce.load();
|
||||||
|
}
|
||||||
|
|
||||||
|
int force() override {
|
||||||
|
return gripperForce.load();
|
||||||
|
}
|
||||||
|
|
||||||
|
int speed() override {
|
||||||
|
return gripperSpeed.load();
|
||||||
|
}
|
||||||
|
|
||||||
|
int position() override {
|
||||||
|
return gripperPosition.load();
|
||||||
|
}
|
||||||
|
|
||||||
|
bool is_active() override {
|
||||||
|
return isActive.load();
|
||||||
|
}
|
||||||
|
|
||||||
|
void activate(bool autoCalibrate = true) override {
|
||||||
|
isActive.store(true);
|
||||||
|
}
|
||||||
|
|
||||||
|
void deactivate() override {
|
||||||
|
isActive.store(false);
|
||||||
|
}
|
||||||
|
|
||||||
|
std::tuple<bool, int> move(int targetPosition, int speed, int force) override{
|
||||||
|
// Ensure only one movement operation is active at a time
|
||||||
|
if (movementThread && movementThread->joinable()) {
|
||||||
|
movementThread->join(); // Wait for any ongoing movement to complete
|
||||||
|
}
|
||||||
|
|
||||||
|
// Start a new movement operation in a background thread
|
||||||
|
movementThread = std::make_unique<std::thread>(&MockRobotiq2fSocketAdapter::simulateMovement, this, targetPosition, speed, force);
|
||||||
|
|
||||||
|
return std::make_tuple(true, targetPosition); // Immediately return success and the target position
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::atomic<bool> isConnected;
|
||||||
|
std::atomic<int> gripperPosition;
|
||||||
|
std::atomic<int> gripperForce;
|
||||||
|
std::atomic<int> gripperSpeed;
|
||||||
|
std::atomic<bool> isActive;
|
||||||
|
|
||||||
|
std::unique_ptr<std::thread> movementThread;
|
||||||
|
|
||||||
|
void simulateMovement(int targetPosition, int speed, int force) {
|
||||||
|
int step = (targetPosition > gripperPosition) ? 1 : -1;
|
||||||
|
|
||||||
|
// Simulate moving towards the target position
|
||||||
|
while (gripperPosition != targetPosition) {
|
||||||
|
gripperPosition += step;
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Simulate time to move a step
|
||||||
|
}
|
||||||
|
|
||||||
|
// Here, you simulate setting the speed and force, though for simplicity, we're just logging it
|
||||||
|
// In a real mock, you might update some internal state to reflect these changes
|
||||||
|
std::cout << "Simulated movement to position " << targetPosition << " with speed " << speed << " and force " << force << std::endl;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
} // end namespace
|
||||||
|
|
||||||
|
#endif // MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||||
@ -25,6 +25,7 @@
|
|||||||
#include <cerrno> // For errno
|
#include <cerrno> // For errno
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
|
||||||
|
namespace robotiq{
|
||||||
// Enum for Gripper Status
|
// Enum for Gripper Status
|
||||||
enum class GripperStatus {
|
enum class GripperStatus {
|
||||||
RESET = 0,
|
RESET = 0,
|
||||||
@ -41,11 +42,11 @@ enum class ObjectStatus {
|
|||||||
AT_DEST = 3
|
AT_DEST = 3
|
||||||
};
|
};
|
||||||
|
|
||||||
class Robotiq2fSocketAdapter {
|
class Robotiq2fSocketAdapter : public SocketAdapterBase {
|
||||||
public:
|
public:
|
||||||
|
|
||||||
Robotiq2fSocketAdapter() ;
|
Robotiq2fSocketAdapter() ;
|
||||||
~Robotiq2fSocketAdapter();
|
~Robotiq2fSocketAdapter() override;
|
||||||
|
|
||||||
// Connection management
|
// Connection management
|
||||||
/**
|
/**
|
||||||
@ -60,7 +61,7 @@ public:
|
|||||||
*
|
*
|
||||||
* @throws std::runtime_error if socket creation fails.
|
* @throws std::runtime_error if socket creation fails.
|
||||||
*/
|
*/
|
||||||
bool connect(const std::string& hostname, int port);
|
bool connect(const std::string& hostname, int port) override;
|
||||||
/**
|
/**
|
||||||
* Closes the connection to the Robotiq gripper.
|
* Closes the connection to the Robotiq gripper.
|
||||||
*
|
*
|
||||||
@ -68,7 +69,7 @@ public:
|
|||||||
* is already closed or was never connected, this method has no effect. This method is
|
* is already closed or was never connected, this method has no effect. This method is
|
||||||
* thread-safe and can be called concurrently with other operations on the adapter.
|
* thread-safe and can be called concurrently with other operations on the adapter.
|
||||||
*/
|
*/
|
||||||
void disconnect();
|
void disconnect() override;
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -119,12 +120,9 @@ public:
|
|||||||
int getGripperVariable(const std::string& variable);
|
int getGripperVariable(const std::string& variable);
|
||||||
|
|
||||||
// Gripper status
|
// Gripper status
|
||||||
bool isGripperActive();
|
int force() override;
|
||||||
int getGripperPosition();
|
int speed() override;
|
||||||
int getGripperForce();
|
int position() override;
|
||||||
int force();
|
|
||||||
int speed();
|
|
||||||
int position();
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Checks if the gripper is currently active.
|
* Checks if the gripper is currently active.
|
||||||
@ -137,7 +135,7 @@ public:
|
|||||||
*
|
*
|
||||||
* @throws std::runtime_error if unable to retrieve the gripper's status, typically due to communication issues.
|
* @throws std::runtime_error if unable to retrieve the gripper's status, typically due to communication issues.
|
||||||
*/
|
*/
|
||||||
bool is_active();
|
bool is_active() override;
|
||||||
|
|
||||||
|
|
||||||
// Activates the gripper and optionally performs auto-calibration
|
// Activates the gripper and optionally performs auto-calibration
|
||||||
@ -158,7 +156,7 @@ public:
|
|||||||
* activation command fails, or if the gripper does not reach the expected state within
|
* activation command fails, or if the gripper does not reach the expected state within
|
||||||
* a reasonable timeframe.
|
* a reasonable timeframe.
|
||||||
*/
|
*/
|
||||||
void activate(bool autoCalibrate = true);
|
void activate(bool autoCalibrate = true) override;
|
||||||
/**
|
/**
|
||||||
* Performs auto-calibration of the gripper.
|
* Performs auto-calibration of the gripper.
|
||||||
*
|
*
|
||||||
@ -175,7 +173,7 @@ public:
|
|||||||
* @throws std::runtime_error if the adapter is not connected to the gripper, if the gripper is not in an
|
* @throws std::runtime_error if the adapter is not connected to the gripper, if the gripper is not in an
|
||||||
* active state, or if the calibration sequence fails.
|
* active state, or if the calibration sequence fails.
|
||||||
*/
|
*/
|
||||||
void autoCalibrate(bool log=true);
|
void auto_calibrate(bool log=true) override;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Deactivates the gripper.
|
* Deactivates the gripper.
|
||||||
@ -188,7 +186,7 @@ public:
|
|||||||
*
|
*
|
||||||
* @throws std::runtime_error if the adapter is not connected to the gripper or if the deactivation command fails.
|
* @throws std::runtime_error if the adapter is not connected to the gripper or if the deactivation command fails.
|
||||||
*/
|
*/
|
||||||
void deactivate();
|
void deactivate() override;
|
||||||
|
|
||||||
// Move Commands
|
// Move Commands
|
||||||
/**
|
/**
|
||||||
@ -204,7 +202,7 @@ public:
|
|||||||
*
|
*
|
||||||
* @throws std::runtime_error if there's an error in sending the move command or if the adapter is not connected to a gripper.
|
* @throws std::runtime_error if there's an error in sending the move command or if the adapter is not connected to a gripper.
|
||||||
*/
|
*/
|
||||||
std::tuple<bool, int> move(int position, int speed, int force);
|
std::tuple<bool, int> move(int position, int speed, int force) override;
|
||||||
/**
|
/**
|
||||||
* Commands the gripper to move to a specified position and waits for the move to complete.
|
* Commands the gripper to move to a specified position and waits for the move to complete.
|
||||||
*
|
*
|
||||||
@ -236,19 +234,19 @@ private:
|
|||||||
};
|
};
|
||||||
|
|
||||||
// Smart pointer to manage the socket descriptor with the custom deleter
|
// Smart pointer to manage the socket descriptor with the custom deleter
|
||||||
std::unique_ptr<int, SocketDeleter> _sockfd;
|
std::unique_ptr<int, SocketDeleter> sockfd_;
|
||||||
std::mutex socketMutex; // Mutex for socket access synchronization
|
std::mutex socketMutex_; // Mutex for socket access synchronization
|
||||||
|
|
||||||
// bounds of the encoded gripper states
|
// bounds of the encoded gripper states
|
||||||
int _min_position = 0;
|
int min_position_ = 0;
|
||||||
int _max_position = 255;
|
int max_position_ = 255;
|
||||||
int _min_speed = 0;
|
int min_speed_ = 0;
|
||||||
int _max_speed = 255;
|
int max_speed_ = 255;
|
||||||
int _min_force = 0;
|
int min_force_ = 0;
|
||||||
int _max_force = 255;
|
int max_force_ = 255;
|
||||||
|
|
||||||
int open_position = _min_position;
|
int open_position_ = min_position_;
|
||||||
int closed_position = _max_position;
|
int closed_position_ = max_position_;
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -352,5 +350,6 @@ private:
|
|||||||
// Constants buffer sizes
|
// Constants buffer sizes
|
||||||
const size_t BUFFER_SIZE = 1024; // Adjust as necessary
|
const size_t BUFFER_SIZE = 1024; // Adjust as necessary
|
||||||
};
|
};
|
||||||
|
} // end namespace
|
||||||
|
|
||||||
#endif // ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
#endif // ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||||
|
|||||||
@ -0,0 +1,25 @@
|
|||||||
|
// IGripperAdapter.hpp
|
||||||
|
|
||||||
|
#ifndef SOCKET_ADAPTER_BASE_HPP
|
||||||
|
#define SOCKET_ADAPTER_BASE_HPP
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
#include <tuple>
|
||||||
|
|
||||||
|
namespace robotiq{
|
||||||
|
class SocketAdapterBase {
|
||||||
|
public:
|
||||||
|
virtual ~SocketAdapterBase() = default;
|
||||||
|
|
||||||
|
virtual bool connect(const std::string& hostname, int port) = 0;
|
||||||
|
virtual void disconnect() = 0;
|
||||||
|
virtual int force() = 0;
|
||||||
|
virtual int speed() = 0;
|
||||||
|
virtual int position() = 0;
|
||||||
|
virtual bool is_active() = 0;
|
||||||
|
virtual void activate(bool autoCalibrate = true) = 0;
|
||||||
|
virtual void deactivate() = 0;
|
||||||
|
virtual std::tuple<bool, int> move(int position, int speed, int force) = 0;
|
||||||
|
};
|
||||||
|
} // end namespace
|
||||||
|
#endif // SOCKET_ADAPTER_BASE_HPP
|
||||||
@ -10,18 +10,18 @@
|
|||||||
#include <vector>
|
#include <vector>
|
||||||
#include <atomic>
|
#include <atomic>
|
||||||
|
|
||||||
#include <Robotiq2fSocketAdapter.hpp>
|
#include "robotiq_2f_interface/SocketAdapterBase.hpp"
|
||||||
|
#include "robotiq_2f_interface/Robotiq2fSocketAdapter.hpp"
|
||||||
|
#include "robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp"
|
||||||
|
|
||||||
#include <hardware_interface/actuator_interface.hpp>
|
#include <hardware_interface/actuator_interface.hpp>
|
||||||
#include <hardware_interface/types/hardware_interface_type_values.hpp>
|
#include <hardware_interface/types/hardware_interface_type_values.hpp>
|
||||||
|
|
||||||
#include <rclcpp/rclcpp.hpp>
|
#include <rclcpp/rclcpp.hpp>
|
||||||
#include <urdf/model.h>
|
|
||||||
|
|
||||||
#include <stdexcept>
|
#include <stdexcept>
|
||||||
|
|
||||||
namespace robotiq_driver
|
namespace robotiq{
|
||||||
{
|
|
||||||
class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterface
|
class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterface
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
@ -46,8 +46,9 @@ public:
|
|||||||
|
|
||||||
protected: // Likely changes the access to protected from private
|
protected: // Likely changes the access to protected from private
|
||||||
// Members for driver interaction
|
// Members for driver interaction
|
||||||
std::unique_ptr<Robotiq2fSocketAdapter> socket_adapter_;
|
bool use_mock_hardware_;
|
||||||
// std::unique_ptr<DriverFactory> driver_factory_;
|
std::unique_ptr<SocketAdapterBase> socket_adapter_;
|
||||||
|
void RobotiqGripperHardwareInterface::configureAdapter(bool useMock);
|
||||||
|
|
||||||
// Background communication thread
|
// Background communication thread
|
||||||
std::thread communication_thread_;
|
std::thread communication_thread_;
|
||||||
@ -74,6 +75,8 @@ protected: // Likely changes the access to protected from private
|
|||||||
double gripper_velocity_ = 0.0;
|
double gripper_velocity_ = 0.0;
|
||||||
double gripper_position_command_ = 0.0;
|
double gripper_position_command_ = 0.0;
|
||||||
|
|
||||||
|
rclcpp::Time last_update_time_;
|
||||||
|
|
||||||
double reactivate_gripper_cmd_ = 0.0;
|
double reactivate_gripper_cmd_ = 0.0;
|
||||||
std::atomic<bool> reactivate_gripper_async_cmd_;
|
std::atomic<bool> reactivate_gripper_async_cmd_;
|
||||||
double reactivate_gripper_response_ = 0.0;
|
double reactivate_gripper_response_ = 0.0;
|
||||||
@ -99,6 +102,6 @@ protected: // Likely changes the access to protected from private
|
|||||||
// Logger
|
// Logger
|
||||||
const auto logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface");
|
const auto logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface");
|
||||||
};
|
};
|
||||||
}
|
} // end namespace
|
||||||
|
|
||||||
#endif //ROBOTIQ2F_HARDWARE_INTERFACE_HPP
|
#endif //ROBOTIQ2F_HARDWARE_INTERFACE_HPP
|
||||||
@ -12,7 +12,6 @@
|
|||||||
<depend>rclcpp</depend>
|
<depend>rclcpp</depend>
|
||||||
<depend>rclcpp_lifecycle</depend>
|
<depend>rclcpp_lifecycle</depend>
|
||||||
<depend>hardware_interface</depend>
|
<depend>hardware_interface</depend>
|
||||||
<depend>lifecycle_msgs</depend>
|
|
||||||
<depend>pluginlib</depend>
|
<depend>pluginlib</depend>
|
||||||
|
|
||||||
<test_depend>ament_lint_auto</test_depend>
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
|||||||
@ -1,7 +1,8 @@
|
|||||||
#include "Robotiq2fSocketAdapter.hpp"
|
#include "robotiq_2f_interface/Robotiq2fSocketAdapter.hpp"
|
||||||
|
|
||||||
|
|
||||||
Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : _sockfd(new int(-1)) {
|
namespace robotiq{
|
||||||
|
Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : sockfd_(new int(-1)) {
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -11,8 +12,8 @@ Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() {
|
|||||||
|
|
||||||
// Connection and disconnection
|
// Connection and disconnection
|
||||||
bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
|
bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
|
||||||
std::lock_guard<std::mutex> lock(socketMutex);
|
std::lock_guard<std::mutex> lock(socketMutex_);
|
||||||
if (*_sockfd >= 0) {
|
if (*sockfd_ >= 0) {
|
||||||
std::cerr << "Already connected. Disconnecting to reconnect.\n";
|
std::cerr << "Already connected. Disconnecting to reconnect.\n";
|
||||||
disconnect();
|
disconnect();
|
||||||
}
|
}
|
||||||
@ -22,7 +23,7 @@ bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
|
|||||||
std::cerr << "Socket creation failed: " << strerror(errno) << "\n";
|
std::cerr << "Socket creation failed: " << strerror(errno) << "\n";
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
*_sockfd = sock;
|
*sockfd_ = sock;
|
||||||
|
|
||||||
sockaddr_in serv_addr{};
|
sockaddr_in serv_addr{};
|
||||||
memset(&serv_addr, 0, sizeof(serv_addr));
|
memset(&serv_addr, 0, sizeof(serv_addr));
|
||||||
@ -31,15 +32,15 @@ bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
|
|||||||
|
|
||||||
if (inet_pton(AF_INET, hostname.c_str(), &serv_addr.sin_addr) <= 0) {
|
if (inet_pton(AF_INET, hostname.c_str(), &serv_addr.sin_addr) <= 0) {
|
||||||
std::cerr << "Invalid address: " << hostname << "\n";
|
std::cerr << "Invalid address: " << hostname << "\n";
|
||||||
close(*_sockfd);
|
close(*sockfd_);
|
||||||
*_sockfd = -1;
|
*sockfd_ = -1;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (::connect(_sockfd, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) {
|
if (::connect(sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) {
|
||||||
std::cerr << "Connection to " << hostname << ":" << port << " failed: " << strerror(errno) << "\n";
|
std::cerr << "Connection to " << hostname << ":" << port << " failed: " << strerror(errno) << "\n";
|
||||||
close(*_sockfd);
|
close(*sockfd_);
|
||||||
*_sockfd = -1;
|
*sockfd_ = -1;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -47,26 +48,26 @@ bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void Robotiq2fSocketAdapter::disconnect() {
|
void Robotiq2fSocketAdapter::disconnect() {
|
||||||
std::lock_guard<std::mutex> lock(socketMutex);
|
std::lock_guard<std::mutex> lock(socketMutex_);
|
||||||
if (*_sockfd >= 0) {
|
if (*sockfd_ >= 0) {
|
||||||
if (close(*_sockfd) == -1) {
|
if (close(*sockfd_) == -1) {
|
||||||
std::cerr << "Error closing socket: " << strerror(errno) << "\n";
|
std::cerr << "Error closing socket: " << strerror(errno) << "\n";
|
||||||
} else {
|
} else {
|
||||||
std::cout << "Disconnected successfully.\n";
|
std::cout << "Disconnected successfully.\n";
|
||||||
}
|
}
|
||||||
*_sockfd = -1;
|
*sockfd_ = -1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Utility methods
|
// Utility methods
|
||||||
bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
|
bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
|
||||||
std::lock_guard<std::mutex> lock(socketMutex);
|
std::lock_guard<std::mutex> lock(socketMutex_);
|
||||||
if (*_sockfd < 0) {
|
if (*sockfd_ < 0) {
|
||||||
std::cerr << "Attempted to send command on a disconnected socket.\n";
|
std::cerr << "Attempted to send command on a disconnected socket.\n";
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
ssize_t result = send(*_sockfd, cmd.c_str(), cmd.length(), 0);
|
ssize_t result = send(*sockfd_, cmd.c_str(), cmd.length(), 0);
|
||||||
if (result < 0) {
|
if (result < 0) {
|
||||||
std::cerr << "Failed to send command: " << strerror(errno) << "\n";
|
std::cerr << "Failed to send command: " << strerror(errno) << "\n";
|
||||||
return false;
|
return false;
|
||||||
@ -76,8 +77,8 @@ bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms=2000) {
|
std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms=2000) {
|
||||||
std::lock_guard<std::mutex> lock(socketMutex);
|
std::lock_guard<std::mutex> lock(socketMutex_);
|
||||||
if (*_sockfd < 0) {
|
if (*sockfd_ < 0) {
|
||||||
std::cerr << "Attempted to receive response on a disconnected socket.\n";
|
std::cerr << "Attempted to receive response on a disconnected socket.\n";
|
||||||
return "";
|
return "";
|
||||||
}
|
}
|
||||||
@ -94,9 +95,9 @@ std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms=2000) {
|
|||||||
while (!complete_response) {
|
while (!complete_response) {
|
||||||
fd_set readfds;
|
fd_set readfds;
|
||||||
FD_ZERO(&readfds);
|
FD_ZERO(&readfds);
|
||||||
FD_SET(*_sockfd, &readfds);
|
FD_SET(*sockfd_, &readfds);
|
||||||
|
|
||||||
int activity = select(*_sockfd + 1, &readfds, nullptr, nullptr, &tv);
|
int activity = select(*sockfd_ + 1, &readfds, nullptr, nullptr, &tv);
|
||||||
if (activity == -1) {
|
if (activity == -1) {
|
||||||
std::cerr << "[receiveResponse] Error in select(): " << strerror(errno) << "\n";
|
std::cerr << "[receiveResponse] Error in select(): " << strerror(errno) << "\n";
|
||||||
return "";
|
return "";
|
||||||
@ -105,7 +106,7 @@ std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms=2000) {
|
|||||||
return "";
|
return "";
|
||||||
}
|
}
|
||||||
|
|
||||||
ssize_t bytes_read = recv(*_sockfd, buffer, BUFFER_SIZE - 1, 0);
|
ssize_t bytes_read = recv(*sockfd_, buffer, BUFFER_SIZE - 1, 0);
|
||||||
if (bytes_read < 0) {
|
if (bytes_read < 0) {
|
||||||
if (errno != EAGAIN && errno != EWOULDBLOCK) {
|
if (errno != EAGAIN && errno != EWOULDBLOCK) {
|
||||||
std::cerr << "Failed to receive response: " << strerror(errno) << "\n";
|
std::cerr << "Failed to receive response: " << strerror(errno) << "\n";
|
||||||
@ -166,8 +167,8 @@ bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, dou
|
|||||||
cmd += "\n";
|
cmd += "\n";
|
||||||
|
|
||||||
// Send the command and receive the response
|
// Send the command and receive the response
|
||||||
std::lock_guard<std::mutex> lock(socketMutex);
|
std::lock_guard<std::mutex> lock(socketMutex_);
|
||||||
if (*_sockfd < 0) {
|
if (*sockfd_ < 0) {
|
||||||
throw std::runtime_error("Cannot set variables on a disconnected socket.");
|
throw std::runtime_error("Cannot set variables on a disconnected socket.");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
@ -191,8 +192,8 @@ bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, dou
|
|||||||
|
|
||||||
std::string cmd = SET_COMMAND + " " + commandVariables.at(variable) + " " + ss.str() + "\n";
|
std::string cmd = SET_COMMAND + " " + commandVariables.at(variable) + " " + ss.str() + "\n";
|
||||||
|
|
||||||
std::lock_guard<std::mutex> lock(socketMutex);
|
std::lock_guard<std::mutex> lock(socketMutex_);
|
||||||
if (*_sockfd < 0) {
|
if (*sockfd_ < 0) {
|
||||||
throw std::runtime_error("Cannot set variables on a disconnected socket.");
|
throw std::runtime_error("Cannot set variables on a disconnected socket.");
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
@ -209,8 +210,8 @@ bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, dou
|
|||||||
int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
|
int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
|
||||||
std::string cmd = GET_COMMAND + " " + commandVariables.at(variable) + "\n";
|
std::string cmd = GET_COMMAND + " " + commandVariables.at(variable) + "\n";
|
||||||
|
|
||||||
std::lock_guard<std::mutex> lock(socketMutex);
|
std::lock_guard<std::mutex> lock(socketMutex_);
|
||||||
if (*_sockfd < 0) {
|
if (*sockfd_ < 0) {
|
||||||
throw std::runtime_error("Cannot get variables on a disconnected socket.");
|
throw std::runtime_error("Cannot get variables on a disconnected socket.");
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
@ -288,9 +289,9 @@ int Robotiq2fSocketAdapter::position(){
|
|||||||
|
|
||||||
// Movement Methods
|
// Movement Methods
|
||||||
std::tuple<bool, int> Robotiq2fSocketAdapter::move(int position, int speed, int force) {
|
std::tuple<bool, int> Robotiq2fSocketAdapter::move(int position, int speed, int force) {
|
||||||
int clippedPosition = clip_val(_min_position, position, _max_position);
|
int clippedPosition = clip_val(min_position_, position, max_position_);
|
||||||
int clippedSpeed = clip_val(_min_speed, speed, _max_speed);
|
int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
|
||||||
int clippedForce = clip_val(_min_force, force, _max_force);
|
int clippedForce = clip_val(min_force_, force, max_force_);
|
||||||
|
|
||||||
// Create a map for gripper variables (similar to Python's OrderedDict)
|
// Create a map for gripper variables (similar to Python's OrderedDict)
|
||||||
std::map<std::string, int> variableMap = {
|
std::map<std::string, int> variableMap = {
|
||||||
@ -305,9 +306,9 @@ std::tuple<bool, int> Robotiq2fSocketAdapter::move(int position, int speed, int
|
|||||||
}
|
}
|
||||||
|
|
||||||
std::tuple<int, ObjectStatus> Robotiq2fSocketAdapter::move_and_wait_for_pos(int position, int speed, int force) {
|
std::tuple<int, ObjectStatus> Robotiq2fSocketAdapter::move_and_wait_for_pos(int position, int speed, int force) {
|
||||||
int clippedPosition = clip_val(min_position, position, max_position);
|
int clippedPosition = clip_val(min_position_, position, max_position_);
|
||||||
int clippedSpeed = clip_val(min_speed, speed, max_speed);
|
int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
|
||||||
int clippedForce = clip_val(min_force, force, max_force);
|
int clippedForce = clip_val(min_force_, force, max_force_);
|
||||||
|
|
||||||
// Set gripper variables to initiate the move
|
// Set gripper variables to initiate the move
|
||||||
std::map<std::string, int> variableMap = {
|
std::map<std::string, int> variableMap = {
|
||||||
@ -353,17 +354,18 @@ void Robotiq2fSocketAdapter::auto_calibrate(bool log=true) {
|
|||||||
if (std::get<1>(result) != ObjectStatus::AT_DEST) {
|
if (std::get<1>(result) != ObjectStatus::AT_DEST) {
|
||||||
throw std::runtime_error("Calibration failed because of an object.");
|
throw std::runtime_error("Calibration failed because of an object.");
|
||||||
}
|
}
|
||||||
_max_position = std::get<0>(result); // Update max position
|
max_position_ = std::get<0>(result); // Update max position
|
||||||
|
|
||||||
// Open as far as possible
|
// Open as far as possible
|
||||||
result = move_and_wait_for_pos(open_position, 64, 1);
|
result = move_and_wait_for_pos(open_position, 64, 1);
|
||||||
if (std::get<1>(result) != ObjectStatus::AT_DEST) {
|
if (std::get<1>(result) != ObjectStatus::AT_DEST) {
|
||||||
throw std::runtime_error("Calibration failed because of an object.");
|
throw std::runtime_error("Calibration failed because of an object.");
|
||||||
}
|
}
|
||||||
_min_position = std::get<0>(result); // Update min position
|
min_position_ = std::get<0>(result); // Update min position
|
||||||
|
|
||||||
if (log) {
|
if (log) {
|
||||||
std::cout << "Gripper auto-calibrated to ["
|
std::cout << "Gripper auto-calibrated to ["
|
||||||
<< _min_position << ", " << _max_position << "]\n";
|
<< min_position_ << ", " << max_position_ << "]\n";
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
} // end namespace
|
||||||
@ -1,11 +1,10 @@
|
|||||||
#include "RobotiqGripperHardwareInterface.hpp"
|
#include "robotiq_2f_interface/hardware_interface.hpp"
|
||||||
|
|
||||||
namespace robotiq_driver
|
namespace robotiq
|
||||||
{
|
{
|
||||||
RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface()
|
RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface()
|
||||||
: communication_thread_is_running_(false)
|
: communication_thread_is_running_(false)
|
||||||
{
|
{
|
||||||
// socket_factory_ = std::make_unique<DefaultSocketFactory>();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface()
|
RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface()
|
||||||
@ -41,6 +40,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
|
|||||||
{
|
{
|
||||||
robot_ip_ = info_.hardware_parameters.at("robot_ip");
|
robot_ip_ = info_.hardware_parameters.at("robot_ip");
|
||||||
robot_port_ = std::stod(info_.hardware_parameters.at("robot_port"));
|
robot_port_ = std::stod(info_.hardware_parameters.at("robot_port"));
|
||||||
|
use_mock_hardware_ = std::stod(info_.hardware_parameters.at("use_fake_hardware"));
|
||||||
node->get_parameter("min_position", min_position_);
|
node->get_parameter("min_position", min_position_);
|
||||||
node->get_parameter("max_position", max_position_);
|
node->get_parameter("max_position", max_position_);
|
||||||
node->get_parameter("min_speed", min_speed_);
|
node->get_parameter("min_speed", min_speed_);
|
||||||
@ -98,8 +98,6 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Socket Factory -> not added yet
|
|
||||||
|
|
||||||
return CallbackReturn::SUCCESS;
|
return CallbackReturn::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -249,7 +247,7 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclc
|
|||||||
|
|
||||||
// Fetch current position and other states from the hardware
|
// Fetch current position and other states from the hardware
|
||||||
try {
|
try {
|
||||||
int raw_position = socket_adapter_->getGripperPosition(); // This should be an existing method in your adapter
|
int raw_position = socket_adapter_->position(); // This should be an existing method in your adapter
|
||||||
double new_position = convertRawToPosition(raw_position);
|
double new_position = convertRawToPosition(raw_position);
|
||||||
rclcpp::Time current_time = time;
|
rclcpp::Time current_time = time;
|
||||||
|
|
||||||
@ -309,6 +307,7 @@ void RobotiqGripperHardwareInterface::background_task()
|
|||||||
{
|
{
|
||||||
while (communication_thread_is_running_.load())
|
while (communication_thread_is_running_.load())
|
||||||
{
|
{
|
||||||
|
std::lock_guard<std::mutex> guard(resource_mutex);
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
// Re-activate the gripper
|
// Re-activate the gripper
|
||||||
@ -331,10 +330,10 @@ void RobotiqGripperHardwareInterface::background_task()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Read the state of the gripper.
|
// Read the state of the gripper.
|
||||||
int raw_position = socket_adapter_->getGripperPosition();
|
int raw_position = socket_adapter_->position();
|
||||||
gripper_current_state_.store(convertRawToPosition(raw_position));
|
gripper_current_state_.store(convertRawToPosition(raw_position));
|
||||||
}
|
}
|
||||||
catch (std::exception& e)
|
catch (const std::exception& e)
|
||||||
{
|
{
|
||||||
RCLCPP_ERROR(logger_, "Error in background task: %s", e.what());
|
RCLCPP_ERROR(logger_, "Error in background task: %s", e.what());
|
||||||
}
|
}
|
||||||
@ -344,6 +343,17 @@ void RobotiqGripperHardwareInterface::background_task()
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Mock Hardware
|
||||||
|
void RobotiqGripperHardwareInterface::configureAdapter(bool useMock) {
|
||||||
|
if (useMock) {
|
||||||
|
RCLCPP_INFO(logger_, "Using mock Robotiq adapter");
|
||||||
|
socket_adapter_ = std::make_unique<MockRobotiq2fSocketAdapter>();
|
||||||
|
} else {
|
||||||
|
RCLCPP_INFO(logger_, "Using real Robotiq adapter");
|
||||||
|
socket_adapter_ = std::make_unique<Robotiq2fSocketAdapter>();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
// Conversion methods
|
// Conversion methods
|
||||||
double RobotiqGripperHardwareInterface::convertRawToPosition(int raw_position) {
|
double RobotiqGripperHardwareInterface::convertRawToPosition(int raw_position) {
|
||||||
|
|||||||
9
src/robotiq_2f/robotiq_controllers/CHANGELOG.rst
Normal file
9
src/robotiq_2f/robotiq_controllers/CHANGELOG.rst
Normal file
@ -0,0 +1,9 @@
|
|||||||
|
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||||
|
Changelog for package robotiq_controllers
|
||||||
|
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||||
|
|
||||||
|
0.0.1 (2023-07-17)
|
||||||
|
------------------
|
||||||
|
* Initial ROS 2 release of robotiq_controllers
|
||||||
|
* This package is not supported by Robotiq but is being maintained by PickNik Robotics
|
||||||
|
* Contributors: Alex Moriarty, Cory Crean
|
||||||
84
src/robotiq_2f/robotiq_controllers/CMakeLists.txt
Normal file
84
src/robotiq_2f/robotiq_controllers/CMakeLists.txt
Normal file
@ -0,0 +1,84 @@
|
|||||||
|
cmake_minimum_required(VERSION 3.8)
|
||||||
|
project(robotiq_controllers)
|
||||||
|
|
||||||
|
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(controller_interface REQUIRED)
|
||||||
|
find_package(std_srvs REQUIRED)
|
||||||
|
|
||||||
|
set(THIS_PACKAGE_INCLUDE_DEPENDS
|
||||||
|
controller_interface
|
||||||
|
std_srvs
|
||||||
|
)
|
||||||
|
|
||||||
|
include_directories(include)
|
||||||
|
|
||||||
|
add_library(${PROJECT_NAME} SHARED
|
||||||
|
src/robotiq_activation_controller.cpp
|
||||||
|
)
|
||||||
|
|
||||||
|
target_include_directories(${PROJECT_NAME} PRIVATE
|
||||||
|
include
|
||||||
|
)
|
||||||
|
|
||||||
|
ament_target_dependencies(${PROJECT_NAME}
|
||||||
|
${THIS_PACKAGE_INCLUDE_DEPENDS}
|
||||||
|
)
|
||||||
|
|
||||||
|
pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml)
|
||||||
|
|
||||||
|
# # INSTALL
|
||||||
|
install(
|
||||||
|
TARGETS ${PROJECT_NAME}
|
||||||
|
EXPORT export_${PROJECT_NAME}
|
||||||
|
ARCHIVE DESTINATION lib/${PROJECT_NAME}
|
||||||
|
LIBRARY DESTINATION lib
|
||||||
|
RUNTIME DESTINATION bin
|
||||||
|
)
|
||||||
|
install(
|
||||||
|
DIRECTORY include/
|
||||||
|
DESTINATION include
|
||||||
|
)
|
||||||
|
|
||||||
|
if(BUILD_TESTING)
|
||||||
|
find_package(ament_lint_auto REQUIRED)
|
||||||
|
|
||||||
|
# the following line skips the linter which checks for copyrights
|
||||||
|
# comment the line when a copyright and license is added to all source files
|
||||||
|
set(ament_cmake_copyright_FOUND TRUE)
|
||||||
|
|
||||||
|
# the following line skips cpplint (only works in a git repo)
|
||||||
|
# comment the line when this package is in a git repo and when
|
||||||
|
# a copyright and license is added to all source files
|
||||||
|
set(ament_cmake_cpplint_FOUND TRUE)
|
||||||
|
|
||||||
|
# the following skips uncrustify
|
||||||
|
# ament_uncrustify and ament_clang_format cannot both be satisfied
|
||||||
|
set(ament_cmake_uncrustify_FOUND TRUE)
|
||||||
|
|
||||||
|
# the following skips xmllint
|
||||||
|
# ament_xmllint requires network and can timeout if on throttled networks
|
||||||
|
set(ament_cmake_xmllint_FOUND TRUE)
|
||||||
|
|
||||||
|
ament_lint_auto_find_test_dependencies()
|
||||||
|
endif()
|
||||||
|
|
||||||
|
# # EXPORTS
|
||||||
|
ament_export_include_directories(
|
||||||
|
include
|
||||||
|
)
|
||||||
|
ament_export_libraries(
|
||||||
|
${PROJECT_NAME}
|
||||||
|
)
|
||||||
|
ament_export_targets(
|
||||||
|
export_${PROJECT_NAME}
|
||||||
|
)
|
||||||
|
ament_export_dependencies(
|
||||||
|
${THIS_PACKAGE_INCLUDE_DEPENDS}
|
||||||
|
)
|
||||||
|
|
||||||
|
ament_package()
|
||||||
@ -0,0 +1,7 @@
|
|||||||
|
<library path="robotiq_controllers">
|
||||||
|
<class name="robotiq_controllers/RobotiqActivationController" type="robotiq_controllers::RobotiqActivationController" base_class_type="controller_interface::ControllerInterface">
|
||||||
|
<description>
|
||||||
|
This controller provides an interface to (re-)activate the Robotiq gripper.
|
||||||
|
</description>
|
||||||
|
</class>
|
||||||
|
</library>
|
||||||
@ -0,0 +1,64 @@
|
|||||||
|
// Copyright (c) 2022 PickNik, Inc.
|
||||||
|
//
|
||||||
|
// Redistribution and use in source and binary forms, with or without
|
||||||
|
// modification, are permitted provided that the following conditions are met:
|
||||||
|
//
|
||||||
|
// * Redistributions of source code must retain the above copyright
|
||||||
|
// notice, this list of conditions and the following disclaimer.
|
||||||
|
//
|
||||||
|
// * Redistributions in binary form must reproduce the above copyright
|
||||||
|
// notice, this list of conditions and the following disclaimer in the
|
||||||
|
// documentation and/or other materials provided with the distribution.
|
||||||
|
//
|
||||||
|
// * Neither the name of the {copyright_holder} nor the names of its
|
||||||
|
// contributors may be used to endorse or promote products derived from
|
||||||
|
// this software without specific prior written permission.
|
||||||
|
//
|
||||||
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||||
|
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||||
|
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||||
|
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||||
|
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||||
|
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||||
|
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||||
|
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||||
|
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||||
|
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
// POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "controller_interface/controller_interface.hpp"
|
||||||
|
#include "std_srvs/srv/trigger.hpp"
|
||||||
|
|
||||||
|
namespace robotiq_controllers
|
||||||
|
{
|
||||||
|
class RobotiqActivationController : public controller_interface::ControllerInterface
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
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;
|
||||||
|
|
||||||
|
CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
|
||||||
|
|
||||||
|
CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
|
||||||
|
|
||||||
|
CallbackReturn on_init() override;
|
||||||
|
|
||||||
|
private:
|
||||||
|
bool reactivateGripper(std_srvs::srv::Trigger::Request::SharedPtr req,
|
||||||
|
std_srvs::srv::Trigger::Response::SharedPtr resp);
|
||||||
|
|
||||||
|
static constexpr double ASYNC_WAITING = 2.0;
|
||||||
|
enum CommandInterfaces
|
||||||
|
{
|
||||||
|
REACTIVATE_GRIPPER_CMD,
|
||||||
|
REACTIVATE_GRIPPER_RESPONSE
|
||||||
|
};
|
||||||
|
|
||||||
|
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reactivate_gripper_srv_;
|
||||||
|
};
|
||||||
|
} // namespace robotiq_controllers
|
||||||
23
src/robotiq_2f/robotiq_controllers/package.xml
Normal file
23
src/robotiq_2f/robotiq_controllers/package.xml
Normal file
@ -0,0 +1,23 @@
|
|||||||
|
<?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_controllers</name>
|
||||||
|
<version>0.0.1</version>
|
||||||
|
<description>Controllers for the Robotiq gripper.</description>
|
||||||
|
<maintainer email="alex.moriarty@picknik.ai">Alex Moriarty</maintainer>
|
||||||
|
<maintainer email="marq.rasmussen@picknik.ai">Marq Rasmussen</maintainer>
|
||||||
|
<license>BSD 3-Clause</license>
|
||||||
|
<author>Cory Crean</author>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<depend>controller_interface</depend>
|
||||||
|
<depend>std_srvs</depend>
|
||||||
|
|
||||||
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
@ -0,0 +1,123 @@
|
|||||||
|
// Copyright (c) 2022 PickNik, Inc.
|
||||||
|
//
|
||||||
|
// Redistribution and use in source and binary forms, with or without
|
||||||
|
// modification, are permitted provided that the following conditions are met:
|
||||||
|
//
|
||||||
|
// * Redistributions of source code must retain the above copyright
|
||||||
|
// notice, this list of conditions and the following disclaimer.
|
||||||
|
//
|
||||||
|
// * Redistributions in binary form must reproduce the above copyright
|
||||||
|
// notice, this list of conditions and the following disclaimer in the
|
||||||
|
// documentation and/or other materials provided with the distribution.
|
||||||
|
//
|
||||||
|
// * Neither the name of the {copyright_holder} nor the names of its
|
||||||
|
// contributors may be used to endorse or promote products derived from
|
||||||
|
// this software without specific prior written permission.
|
||||||
|
//
|
||||||
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||||
|
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||||
|
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||||
|
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||||
|
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||||
|
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||||
|
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||||
|
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||||
|
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||||
|
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
// POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
|
||||||
|
#include "robotiq_controllers/robotiq_activation_controller.hpp"
|
||||||
|
|
||||||
|
namespace robotiq_controllers
|
||||||
|
{
|
||||||
|
controller_interface::InterfaceConfiguration RobotiqActivationController::command_interface_configuration() const
|
||||||
|
{
|
||||||
|
controller_interface::InterfaceConfiguration config;
|
||||||
|
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
|
||||||
|
|
||||||
|
config.names.emplace_back("reactivate_gripper/reactivate_gripper_cmd");
|
||||||
|
config.names.emplace_back("reactivate_gripper/reactivate_gripper_response");
|
||||||
|
|
||||||
|
return config;
|
||||||
|
}
|
||||||
|
|
||||||
|
controller_interface::InterfaceConfiguration RobotiqActivationController::state_interface_configuration() const
|
||||||
|
{
|
||||||
|
controller_interface::InterfaceConfiguration config;
|
||||||
|
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
|
||||||
|
|
||||||
|
return config;
|
||||||
|
}
|
||||||
|
|
||||||
|
controller_interface::return_type RobotiqActivationController::update(const rclcpp::Time& /*time*/,
|
||||||
|
const rclcpp::Duration& /*period*/)
|
||||||
|
{
|
||||||
|
return controller_interface::return_type::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
|
RobotiqActivationController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
|
||||||
|
{
|
||||||
|
// Check command interfaces.
|
||||||
|
if (command_interfaces_.size() != 2)
|
||||||
|
{
|
||||||
|
RCLCPP_ERROR(get_node()->get_logger(), "Expected %d command interfaces, but got %zu.", 2,
|
||||||
|
command_interfaces_.size());
|
||||||
|
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
// Create service for re-activating the gripper.
|
||||||
|
reactivate_gripper_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
|
||||||
|
"~/reactivate_gripper",
|
||||||
|
[this](std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp) {
|
||||||
|
this->reactivateGripper(req, resp);
|
||||||
|
});
|
||||||
|
}
|
||||||
|
catch (...)
|
||||||
|
{
|
||||||
|
return LifecycleNodeInterface::CallbackReturn::ERROR;
|
||||||
|
}
|
||||||
|
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
|
}
|
||||||
|
|
||||||
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
|
||||||
|
RobotiqActivationController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
reactivate_gripper_srv_.reset();
|
||||||
|
}
|
||||||
|
catch (...)
|
||||||
|
{
|
||||||
|
return LifecycleNodeInterface::CallbackReturn::ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
|
}
|
||||||
|
|
||||||
|
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqActivationController::on_init()
|
||||||
|
{
|
||||||
|
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool RobotiqActivationController::reactivateGripper(std_srvs::srv::Trigger::Request::SharedPtr /*req*/,
|
||||||
|
std_srvs::srv::Trigger::Response::SharedPtr resp)
|
||||||
|
{
|
||||||
|
command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].set_value(ASYNC_WAITING);
|
||||||
|
command_interfaces_[REACTIVATE_GRIPPER_CMD].set_value(1.0);
|
||||||
|
|
||||||
|
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();
|
||||||
|
|
||||||
|
return resp->success;
|
||||||
|
}
|
||||||
|
} // namespace robotiq_controllers
|
||||||
|
|
||||||
|
#include "pluginlib/class_list_macros.hpp"
|
||||||
|
|
||||||
|
PLUGINLIB_EXPORT_CLASS(robotiq_controllers::RobotiqActivationController, controller_interface::ControllerInterface)
|
||||||
Loading…
Reference in New Issue
Block a user