Compare commits
9 Commits
new_roboti
...
main
| Author | SHA1 | Date | |
|---|---|---|---|
|
6fa800f6d1 | ||
|
|
9295f40d74 | ||
|
|
cf44362587 | ||
|
|
9021f754c9 | ||
|
|
7a3c6fcc2e | ||
|
|
8b3c40bbe7 | ||
|
|
ad299fe757 | ||
| 08bec7a499 | |||
|
|
7ce6aaa707 |
2
.idea/.gitignore
vendored
2
.idea/.gitignore
vendored
@ -6,3 +6,5 @@
|
||||
# Datasource local storage ignored files
|
||||
/dataSources/
|
||||
/dataSources.local.xml
|
||||
# GitHub Copilot persisted chat sessions
|
||||
/copilot/chatSessions
|
||||
|
||||
@ -5,5 +5,7 @@
|
||||
<mapping directory="$PROJECT_DIR$/src/Universal_Robots_ROS2_Description" vcs="Git" />
|
||||
<mapping directory="$PROJECT_DIR$/src/Universal_Robots_ROS2_Driver" vcs="Git" />
|
||||
<mapping directory="$PROJECT_DIR$/src/cartesian_controllers" vcs="Git" />
|
||||
<mapping directory="$PROJECT_DIR$/src/ros2_robotiq_gripper" vcs="Git" />
|
||||
<mapping directory="$PROJECT_DIR$/src/serial" vcs="Git" />
|
||||
</component>
|
||||
</project>
|
||||
25
README.md
25
README.md
@ -78,11 +78,17 @@ ros2 launch ur_robotiq_moveit_config ur_robotiq_moveit.launch.py use_fake_hardwa
|
||||
```
|
||||
|
||||
### servoing
|
||||
#### PS5 controller
|
||||
Terminal 3:\
|
||||
**At the moment the servoing doesnt work with the collision checker (self collision still works).**
|
||||
```bash
|
||||
ros2 launch ur_robotiq_servo ps5_servo.launch.py
|
||||
```
|
||||
#### Keyboard
|
||||
Terminal 3:
|
||||
```bash
|
||||
ros2 run servo_keyboard servo_keyboard_input --ros-args --params-file src/servo_keyboard/config/servo_keyboard_params.yaml
|
||||
```
|
||||
|
||||
After launching you need to change the controller to enable the servo mode.\
|
||||
Terminal 4:
|
||||
@ -91,4 +97,23 @@ ros2 control switch_controllers --deactivate joint_trajectory_controller
|
||||
ros2 control switch_controllers --deactivate robotiq_gripper_joint_trajectory_controller
|
||||
ros2 control switch_controllers --activate forward_position_controller
|
||||
ros2 control switch_controllers --activate forward_gripper_position_controller
|
||||
```
|
||||
|
||||
For the real robot you need to change to the scaled position controller:
|
||||
```bash
|
||||
ros2 control switch_controllers --deactivate scaled_joint_trajectory_controller
|
||||
ros2 control switch_controllers --deactivate robotiq_gripper_joint_trajectory_controller
|
||||
ros2 control switch_controllers --activate scaled_forward_position_controller
|
||||
ros2 control switch_controllers --activate forward_gripper_position_controller
|
||||
```
|
||||
|
||||
Terminal 5:
|
||||
Activate the Servo Node
|
||||
```bash
|
||||
ros2 service call /servo_node/start_servo std_srvs/srv/Trigger {}
|
||||
```
|
||||
|
||||
### extract the urdf from xacro file
|
||||
```bash
|
||||
ros2 run xacro xacro src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro > src/ur_robotiq_description/urdf/ur_robotiq.urdf name:=ur3e
|
||||
```
|
||||
@ -7,7 +7,14 @@ repositories:
|
||||
type: git
|
||||
url: https://github.com/NikoFeith/Universal_Robots_ROS2_Driver.git
|
||||
version: humble
|
||||
serial:
|
||||
type: git
|
||||
url: https://github.com/tylerjw/serial.git
|
||||
version: ros2
|
||||
ros2_robotiq_gripper:
|
||||
type: git
|
||||
url: https://github.com/PickNikRobotics/ros2_robotiq_gripper.git
|
||||
version: main
|
||||
cartesian_controllers:
|
||||
type: git
|
||||
url: https://github.com/NikoFeith/cartesian_controllers.git
|
||||
version: ros2
|
||||
url: https://github.com/NikoFeith/cartesian_controllers.git
|
||||
@ -1,20 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(robotiq_2f_description)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(
|
||||
DIRECTORY
|
||||
config
|
||||
launch
|
||||
meshes
|
||||
rviz
|
||||
urdf
|
||||
DESTINATION
|
||||
share/${PROJECT_NAME}
|
||||
)
|
||||
ament_package()
|
||||
@ -1,8 +0,0 @@
|
||||
robotiq_2f_gripper:
|
||||
# Physical limits
|
||||
min_position: 0.0 # Meters (fully closed)
|
||||
max_position: 0.14 # Meters (fully open)
|
||||
min_speed: 0.02 # Meters per second
|
||||
max_speed: 0.15 # Meters per second
|
||||
min_force: 20.0 # Newtons
|
||||
max_force: 235.0 # Newtons
|
||||
@ -1,8 +0,0 @@
|
||||
robotiq_2f_gripper:
|
||||
# Physical limits
|
||||
min_position: 0.0 # Meters (fully closed)
|
||||
max_position: 0.085 # Meters (fully open)
|
||||
min_speed: 0.02 # Meters per second
|
||||
max_speed: 0.15 # Meters per second
|
||||
min_force: 20.0 # Newtons
|
||||
max_force: 235.0 # Newtons
|
||||
@ -1,20 +0,0 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 500 # Hz
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
robotiq_gripper_controller:
|
||||
type: position_controllers/GripperActionController
|
||||
robotiq_activation_controller:
|
||||
type: robotiq_controllers/RobotiqActivationController
|
||||
|
||||
robotiq_gripper_controller:
|
||||
ros__parameters:
|
||||
default: true
|
||||
joint: finger_joint
|
||||
use_effort_interface: true
|
||||
use_speed_interface: true
|
||||
|
||||
robotiq_activation_controller:
|
||||
ros__parameters:
|
||||
default: true
|
||||
@ -1,148 +0,0 @@
|
||||
# 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.
|
||||
|
||||
import launch
|
||||
from launch.substitutions import (
|
||||
Command,
|
||||
FindExecutable,
|
||||
LaunchConfiguration,
|
||||
PathJoinSubstitution,
|
||||
)
|
||||
from launch.conditions import IfCondition
|
||||
import launch_ros
|
||||
import os
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
description_pkg_share = launch_ros.substitutions.FindPackageShare(
|
||||
package="robotiq_description"
|
||||
).find("robotiq_2f_description")
|
||||
default_model_path = os.path.join(
|
||||
description_pkg_share, "urdf", "robotiq_2f_140.urdf.xacro"
|
||||
)
|
||||
default_rviz_config_path = os.path.join(
|
||||
description_pkg_share, "rviz", "view_urdf.rviz"
|
||||
)
|
||||
|
||||
args = []
|
||||
args.append(
|
||||
launch.actions.DeclareLaunchArgument(
|
||||
name="model",
|
||||
default_value=default_model_path,
|
||||
description="Absolute path to gripper URDF file",
|
||||
)
|
||||
)
|
||||
args.append(
|
||||
launch.actions.DeclareLaunchArgument(
|
||||
name="rvizconfig",
|
||||
default_value=default_rviz_config_path,
|
||||
description="Absolute path to rviz config file",
|
||||
)
|
||||
)
|
||||
args.append(
|
||||
launch.actions.DeclareLaunchArgument(
|
||||
name="launch_rviz", default_value="true", description="Launch RViz?"
|
||||
)
|
||||
)
|
||||
|
||||
robot_description_content = Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
" ",
|
||||
LaunchConfiguration("model"),
|
||||
" ",
|
||||
"use_fake_hardware:=true",
|
||||
]
|
||||
)
|
||||
robot_description_param = {
|
||||
"robot_description": launch_ros.parameter_descriptions.ParameterValue(
|
||||
robot_description_content, value_type=str
|
||||
)
|
||||
}
|
||||
|
||||
controllers_file = "robotiq_controllers.yaml"
|
||||
initial_joint_controllers = PathJoinSubstitution(
|
||||
[description_pkg_share, "config", controllers_file]
|
||||
)
|
||||
|
||||
control_node = launch_ros.actions.Node(
|
||||
package="controller_manager",
|
||||
executable="ros2_control_node",
|
||||
parameters=[
|
||||
robot_description_param,
|
||||
initial_joint_controllers,
|
||||
],
|
||||
)
|
||||
|
||||
robot_state_publisher_node = launch_ros.actions.Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
parameters=[robot_description_param],
|
||||
)
|
||||
|
||||
rviz_node = launch_ros.actions.Node(
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2",
|
||||
output="log",
|
||||
arguments=["-d", LaunchConfiguration("rvizconfig")],
|
||||
condition=IfCondition(LaunchConfiguration("launch_rviz")),
|
||||
)
|
||||
|
||||
joint_state_broadcaster_spawner = launch_ros.actions.Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"joint_state_broadcaster",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
robotiq_gripper_controller_spawner = launch_ros.actions.Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["robotiq_gripper_controller", "-c", "/controller_manager"],
|
||||
)
|
||||
|
||||
robotiq_activation_controller_spawner = launch_ros.actions.Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["robotiq_activation_controller", "-c", "/controller_manager"],
|
||||
)
|
||||
|
||||
nodes = [
|
||||
control_node,
|
||||
robot_state_publisher_node,
|
||||
joint_state_broadcaster_spawner,
|
||||
robotiq_gripper_controller_spawner,
|
||||
robotiq_activation_controller_spawner,
|
||||
rviz_node,
|
||||
]
|
||||
|
||||
return launch.LaunchDescription(args + nodes)
|
||||
@ -1,75 +0,0 @@
|
||||
import launch
|
||||
from launch.substitutions import (
|
||||
Command,
|
||||
FindExecutable,
|
||||
LaunchConfiguration,
|
||||
PathJoinSubstitution,
|
||||
)
|
||||
import launch_ros
|
||||
import os
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
pkg_share = launch_ros.substitutions.FindPackageShare(
|
||||
package="robotiq_2f_description"
|
||||
).find("robotiq_2f_description")
|
||||
default_model_path = os.path.join(
|
||||
pkg_share, "urdf", "robotiq_2f_85.urdf.xacro"
|
||||
)
|
||||
default_rviz_config_path = os.path.join(pkg_share, "rviz", "view_urdf.rviz")
|
||||
|
||||
args = []
|
||||
args.append(
|
||||
launch.actions.DeclareLaunchArgument(
|
||||
name="model",
|
||||
default_value=default_model_path,
|
||||
description="Absolute path to gripper URDF file",
|
||||
)
|
||||
)
|
||||
args.append(
|
||||
launch.actions.DeclareLaunchArgument(
|
||||
name="rvizconfig",
|
||||
default_value=default_rviz_config_path,
|
||||
description="Absolute path to rviz config file",
|
||||
)
|
||||
)
|
||||
|
||||
robot_description_content = Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
" ",
|
||||
LaunchConfiguration("model"),
|
||||
]
|
||||
)
|
||||
robot_description_param = {
|
||||
"robot_description": launch_ros.parameter_descriptions.ParameterValue(
|
||||
robot_description_content, value_type=str
|
||||
)
|
||||
}
|
||||
|
||||
robot_state_publisher_node = launch_ros.actions.Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
parameters=[robot_description_param],
|
||||
)
|
||||
|
||||
joint_state_publisher_node = launch_ros.actions.Node(
|
||||
package="joint_state_publisher_gui",
|
||||
executable="joint_state_publisher_gui",
|
||||
)
|
||||
|
||||
rviz_node = launch_ros.actions.Node(
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2",
|
||||
output="screen",
|
||||
arguments=["-d", LaunchConfiguration("rvizconfig")],
|
||||
)
|
||||
|
||||
nodes = [
|
||||
robot_state_publisher_node,
|
||||
joint_state_publisher_node,
|
||||
rviz_node,
|
||||
]
|
||||
|
||||
return launch.LaunchDescription(args + nodes)
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
@ -1,32 +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_description</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>
|
||||
|
||||
<exec_depend>joint_state_publisher_gui</exec_depend>
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>rviz2</exec_depend>
|
||||
<exec_depend>urdf</exec_depend>
|
||||
<exec_depend>xacro</exec_depend>
|
||||
|
||||
<exec_depend>ros2_control</exec_depend>
|
||||
<exec_depend>ros2_controllers</exec_depend>
|
||||
|
||||
<exec_depend>gripper_controllers</exec_depend>
|
||||
<exec_depend>joint_state_broadcaster</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1,234 +0,0 @@
|
||||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Status1
|
||||
Splitter Ratio: 0.6264705657958984
|
||||
Tree Height: 555
|
||||
- Class: rviz_common/Selection
|
||||
Name: Selection
|
||||
- Class: rviz_common/Tool Properties
|
||||
Expanded:
|
||||
- /2D Goal Pose1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz_common/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz_common/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: ""
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz_default_plugins/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rviz_default_plugins/RobotModel
|
||||
Collision Enabled: false
|
||||
Description File: ""
|
||||
Description Source: Topic
|
||||
Description Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: robot_description
|
||||
Enabled: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
arm_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
dummy_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
end_effector_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
forearm_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
gripper_base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
left_finger_dist_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
left_finger_prox_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
lower_wrist_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_finger_dist_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_finger_prox_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
shoulder_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
table:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
tool_frame:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
upper_wrist_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
world:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Mass Properties:
|
||||
Inertia: false
|
||||
Mass: false
|
||||
Name: RobotModel
|
||||
TF Prefix: ""
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: world
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz_default_plugins/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz_default_plugins/MoveCamera
|
||||
- Class: rviz_default_plugins/Select
|
||||
- Class: rviz_default_plugins/FocusCamera
|
||||
- Class: rviz_default_plugins/Measure
|
||||
Line color: 128; 128; 0
|
||||
- Class: rviz_default_plugins/SetInitialPose
|
||||
Covariance x: 0.25
|
||||
Covariance y: 0.25
|
||||
Covariance yaw: 0.06853891909122467
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /initialpose
|
||||
- Class: rviz_default_plugins/SetGoal
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /goal_pose
|
||||
- Class: rviz_default_plugins/PublishPoint
|
||||
Single click: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /clicked_point
|
||||
Transformation:
|
||||
Current:
|
||||
Class: rviz_default_plugins/TF
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 2.1567115783691406
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: -0.09681572020053864
|
||||
Y: -0.10843408107757568
|
||||
Z: 0.1451336145401001
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.785398006439209
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 0.785398006439209
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 846
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1200
|
||||
X: 1989
|
||||
Y: 261
|
||||
@ -1,97 +0,0 @@
|
||||
<?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
|
||||
fake_sensor_commands
|
||||
robot_ip
|
||||
robot_port
|
||||
min_position
|
||||
max_position
|
||||
min_speed
|
||||
max_speed
|
||||
min_force
|
||||
max_force">
|
||||
|
||||
<ros2_control name="${name}" type="system">
|
||||
<!-- Plugins -->
|
||||
<hardware>
|
||||
<plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
|
||||
<xacro:if value="${use_fake_hardware}">
|
||||
<param name="fake_sensor_commands">${fake_sensor_commands}</param>
|
||||
<param name="state_following_offset">0.0</param>
|
||||
</xacro:if>
|
||||
<param name="gripper_closed_position">0.695</param>
|
||||
<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>
|
||||
</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.695</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<!-- When simulating we need to include the rest of the gripper joints -->
|
||||
<xacro:if value="false">
|
||||
<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>
|
||||
@ -1,13 +0,0 @@
|
||||
<?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>
|
||||
@ -1,437 +0,0 @@
|
||||
<?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">
|
||||
|
||||
|
||||
<xacro:property name="config_file" value="$(find robotiq_2f_description)/config/robotiq_2f_140_config.yaml" />
|
||||
<xacro:property name="config_dict" value="${xacro.load_yaml(config_file)}" />
|
||||
<xacro:property name="robotiq_2f_gripper_dict" value="${config_dict['robotiq_2f_gripper']}" />
|
||||
<xacro:property name="min_position" value="${robotiq_2f_gripper_dict['min_position']}" />
|
||||
<xacro:property name="max_position" value="${robotiq_2f_gripper_dict['max_position']}" />
|
||||
<xacro:property name="min_speed" value="${robotiq_2f_gripper_dict['min_speed']}" />
|
||||
<xacro:property name="max_speed" value="${robotiq_2f_gripper_dict['max_speed']}" />
|
||||
<xacro:property name="min_force" value="${robotiq_2f_gripper_dict['min_force']}" />
|
||||
<xacro:property name="max_force" value="${robotiq_2f_gripper_dict['max_force']}" />
|
||||
<!-- ros2 control include -->
|
||||
<xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_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}"
|
||||
min_position="${min_position}"
|
||||
max_position="${max_position}"
|
||||
min_speed="${min_speed}"
|
||||
max_speed="${max_speed}"
|
||||
min_force="${min_force}"
|
||||
max_force="${max_force}"/>
|
||||
</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>
|
||||
@ -1,100 +0,0 @@
|
||||
<?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
|
||||
fake_sensor_commands
|
||||
robot_ip
|
||||
robot_port
|
||||
min_position
|
||||
max_position
|
||||
min_speed
|
||||
max_speed
|
||||
min_force
|
||||
max_force">
|
||||
|
||||
<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="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>
|
||||
</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>
|
||||
@ -1,13 +0,0 @@
|
||||
<?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_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>
|
||||
@ -1,307 +0,0 @@
|
||||
<?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">
|
||||
|
||||
<xacro:property name="config_file" value="$(find robotiq_2f_description)/config/robotiq_2f_85_config.yaml" />
|
||||
<xacro:property name="config_dict" value="${xacro.load_yaml(config_file)}" />
|
||||
<xacro:property name="robotiq_2f_gripper_dict" value="${config_dict['robotiq_2f_gripper']}" />
|
||||
<xacro:property name="min_position" value="${robotiq_2f_gripper_dict['min_position']}" />
|
||||
<xacro:property name="max_position" value="${robotiq_2f_gripper_dict['max_position']}" />
|
||||
<xacro:property name="min_speed" value="${robotiq_2f_gripper_dict['min_speed']}" />
|
||||
<xacro:property name="max_speed" value="${robotiq_2f_gripper_dict['max_speed']}" />
|
||||
<xacro:property name="min_force" value="${robotiq_2f_gripper_dict['min_force']}" />
|
||||
<xacro:property name="max_force" value="${robotiq_2f_gripper_dict['max_force']}" />
|
||||
|
||||
<!-- ros2 control include -->
|
||||
<xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_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}"
|
||||
min_position="${min_position}"
|
||||
max_position="${max_position}"
|
||||
min_speed="${min_speed}"
|
||||
max_speed="${max_speed}"
|
||||
min_force="${min_force}"
|
||||
max_force="${max_force}"/>
|
||||
</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,48 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(robotiq_2f_interface)
|
||||
|
||||
# Default to C++17
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
endif()
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(hardware_interface REQUIRED)
|
||||
find_package(pluginlib REQUIRED)
|
||||
|
||||
include_directories(include)
|
||||
|
||||
# Declare a C++ library
|
||||
add_library(${PROJECT_NAME} SHARED
|
||||
src/hardware_interface.cpp
|
||||
src/Robotiq2fSocketAdapter.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(${PROJECT_NAME}
|
||||
rclcpp
|
||||
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()
|
||||
@ -1,9 +0,0 @@
|
||||
<library path="robotiq_2f_interface">
|
||||
<class name="robotiq_2f_interface/RobotiqGripperHardwareInterface"
|
||||
type="robotiq_interface::RobotiqGripperHardwareInterface"
|
||||
base_class_type="hardware_interface::SystemInterface">
|
||||
<description>
|
||||
ROS2 controller for the Robotiq gripper.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
@ -1,95 +0,0 @@
|
||||
// MockRobotiq2fSocketAdapter.hpp
|
||||
|
||||
#ifndef MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||
#define MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include <atomic>
|
||||
#include <tuple>
|
||||
|
||||
#include "robotiq_2f_interface/SocketAdapterBase.hpp"
|
||||
|
||||
namespace robotiq_interface
|
||||
{
|
||||
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 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 robotiq_interface
|
||||
|
||||
#endif // MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||
@ -1,360 +0,0 @@
|
||||
// Robotiq2fSocketAdapter.hpp
|
||||
|
||||
#ifndef ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||
#define ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||
|
||||
#include <mutex>
|
||||
#include <thread>
|
||||
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include <vector>
|
||||
#include <sys/time.h>
|
||||
#include <sstream>
|
||||
#include <tuple>
|
||||
|
||||
#include <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
#include <arpa/inet.h>
|
||||
|
||||
#include <unistd.h> // For close()
|
||||
#include <cstring> // For memset()
|
||||
#include <sys/select.h> // For select() system call
|
||||
|
||||
#include <stdexcept> // For std::runtime_error
|
||||
#include <cerrno> // For errno
|
||||
#include <iostream>
|
||||
|
||||
#include "robotiq_2f_interface/SocketAdapterBase.hpp"
|
||||
|
||||
namespace robotiq_interface
|
||||
{
|
||||
// Enum for Gripper Status
|
||||
enum class GripperStatus {
|
||||
RESET = 0,
|
||||
ACTIVATING = 1,
|
||||
// UNUSED = 2, // This value is currently not used by the gripper firmware
|
||||
ACTIVE = 3
|
||||
};
|
||||
|
||||
// Enum for Object Status
|
||||
enum class ObjectStatus {
|
||||
MOVING = 0,
|
||||
STOPPED_OUTER_OBJECT = 1,
|
||||
STOPPED_INNER_OBJECT = 2,
|
||||
AT_DEST = 3
|
||||
};
|
||||
|
||||
class Robotiq2fSocketAdapter : public SocketAdapterBase {
|
||||
public:
|
||||
|
||||
Robotiq2fSocketAdapter() ;
|
||||
~Robotiq2fSocketAdapter() override;
|
||||
|
||||
// Connection management
|
||||
/**
|
||||
* Establishes a TCP connection to the Robotiq gripper.
|
||||
*
|
||||
* Attempts to open a socket and connect to the specified hostname and port. If the adapter
|
||||
* is already connected, it will disconnect and attempt to reconnect.
|
||||
*
|
||||
* @param hostname The IP address or hostname of the Robotiq gripper.
|
||||
* @param port The port number on which the gripper is listening for connections.
|
||||
* @return True if the connection was successfully established, False otherwise.
|
||||
*
|
||||
* @throws std::runtime_error if socket creation fails.
|
||||
*/
|
||||
bool connect(const std::string& hostname, int port) override;
|
||||
/**
|
||||
* Closes the connection to the Robotiq gripper.
|
||||
*
|
||||
* Safely shuts down and closes the current socket connection to the gripper. If the socket
|
||||
* 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.
|
||||
*/
|
||||
void disconnect() override;
|
||||
|
||||
|
||||
/**
|
||||
* Sets multiple gripper variables using a map of variable names to integer/double values.
|
||||
*
|
||||
* This method allows setting multiple configuration parameters of the gripper simultaneously.
|
||||
* The variable names must match the expected variable identifiers used by the gripper.
|
||||
* Integer/double values are used directly in the command sent to the gripper.
|
||||
*
|
||||
* @param variableMap A map where each key is a string representing the variable name and
|
||||
* each value is an integer representing the variable's desired value.
|
||||
* @return True if the command was successfully sent and acknowledged by the gripper, False otherwise.
|
||||
*
|
||||
* @throws std::runtime_error if the adapter is not connected to the gripper or if sending the command fails.
|
||||
*/
|
||||
bool setGripperVariables(const std::map<std::string, int>& variableMap);
|
||||
bool setGripperVariables(const std::map<std::string, double>& variableMap);
|
||||
|
||||
/**
|
||||
* Sets a single gripper variable.
|
||||
*
|
||||
* Allows setting a single configuration parameter of the gripper. The method overloads allow
|
||||
* specifying the variable value as either an integer or a double, depending on the expected
|
||||
* variable type.
|
||||
*
|
||||
* @param variable The variable name, matching the gripper's expected variable identifiers.
|
||||
* @param value The desired value for the variable, either as an int or a double.
|
||||
* @return True if the command was successfully sent and acknowledged by the gripper, False otherwise.
|
||||
*
|
||||
* @throws std::runtime_error if the adapter is not connected to the gripper or if sending the command fails.
|
||||
*/
|
||||
bool setGripperVariable(const std::string& variable, int value);
|
||||
bool setGripperVariable(const std::string& variable, double value);
|
||||
|
||||
/**
|
||||
* Retrieves the current value of a specified gripper variable.
|
||||
*
|
||||
* Sends a command to the gripper to report the current value of a specific configuration parameter.
|
||||
* The method parses the gripper's response to extract the variable value.
|
||||
*
|
||||
* @param variable The variable name, matching the gripper's expected variable identifiers.
|
||||
* @return The current value of the variable as an integer. Returns -1 if the command fails or
|
||||
* if the variable is not recognized by the gripper.
|
||||
*
|
||||
* @throws std::runtime_error if the adapter is not connected to the gripper, if sending the command fails,
|
||||
* or if the response from the gripper is malformed.
|
||||
*/
|
||||
int getGripperVariable(const std::string& variable);
|
||||
|
||||
// Gripper status
|
||||
int force() override;
|
||||
int speed() override;
|
||||
int position() override;
|
||||
|
||||
/**
|
||||
* Checks if the gripper is currently active.
|
||||
*
|
||||
* Queries the current state of the gripper to determine if it is active. This method interprets
|
||||
* the gripper's status based on the last known communication and does not initiate a new request
|
||||
* to the gripper.
|
||||
*
|
||||
* @return True if the gripper is in the ACTIVE state, False otherwise.
|
||||
*
|
||||
* @throws std::runtime_error if unable to retrieve the gripper's status, typically due to communication issues.
|
||||
*/
|
||||
bool is_active() override;
|
||||
|
||||
|
||||
// Activates the gripper and optionally performs auto-calibration
|
||||
/**
|
||||
* Activates the Robotiq gripper and optionally performs auto-calibration.
|
||||
*
|
||||
* This method sends an activation command to the gripper, bringing it from a RESET or INACTIVE
|
||||
* state to an ACTIVE state. If autoCalibrate is set to true, it also initiates an auto-calibration
|
||||
* procedure to determine the maximum and minimum positions of the gripper's motion range.
|
||||
*
|
||||
* @param autoCalibrate A boolean flag indicating whether to perform auto-calibration after
|
||||
* activation. Default is true.
|
||||
*
|
||||
* @note Activation and auto-calibration may take several seconds to complete. The method
|
||||
* waits for the gripper to acknowledge activation before returning.
|
||||
*
|
||||
* @throws std::runtime_error if the adapter is not connected to the gripper, if sending the
|
||||
* activation command fails, or if the gripper does not reach the expected state within
|
||||
* a reasonable timeframe.
|
||||
*/
|
||||
void activate(bool autoCalibrate = true) override;
|
||||
/**
|
||||
* Performs auto-calibration of the gripper.
|
||||
*
|
||||
* This method initiates an auto-calibration sequence where the gripper fully opens and closes to determine
|
||||
* its operational range. This is useful for ensuring accurate position control, especially if the mechanical
|
||||
* setup of the gripper has been altered or if it is being used for the first time.
|
||||
*
|
||||
* @param log A boolean flag indicating whether to log the calibration results. Defaults to true. When set
|
||||
* to true, the final calibration values will be logged to the standard output.
|
||||
*
|
||||
* @note This method assumes the gripper is already activated. If the gripper is not in an active state,
|
||||
* the calibration sequence may fail. Ensure the gripper is activated before calling this method.
|
||||
*
|
||||
* @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.
|
||||
*/
|
||||
void auto_calibrate(bool log=true);
|
||||
|
||||
/**
|
||||
* Deactivates the gripper.
|
||||
*
|
||||
* Sends a command to transition the gripper from an active state back to a reset state. This method
|
||||
* is useful for safely shutting down the gripper or preparing it for a state change.
|
||||
*
|
||||
* @note This method should be called before disconnecting from the gripper or when the gripper is not
|
||||
* needed for an extended period to ensure it is left in a safe state.
|
||||
*
|
||||
* @throws std::runtime_error if the adapter is not connected to the gripper or if the deactivation command fails.
|
||||
*/
|
||||
void deactivate() override;
|
||||
|
||||
// Move Commands
|
||||
/**
|
||||
* Commands the gripper to move to a specified position.
|
||||
*
|
||||
* Instructs the gripper to move to a given position with specified speed and force. The position,
|
||||
* speed, and force parameters are automatically clipped to the valid range supported by the gripper.
|
||||
*
|
||||
* @param position The target position for the gripper, typically in the range [0, 255].
|
||||
* @param speed The speed at which the gripper should move, typically in the range [0, 255].
|
||||
* @param force The force to be applied by the gripper, typically in the range [0, 255].
|
||||
* @return A tuple containing a boolean indicating success of the command, and an integer representing the clipped target position.
|
||||
*
|
||||
* @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) override;
|
||||
/**
|
||||
* Commands the gripper to move to a specified position and waits for the move to complete.
|
||||
*
|
||||
* This method instructs the gripper to move to the given position with specified speed and force,
|
||||
* then blocks until the gripper acknowledges reaching the target position or stops moving due to
|
||||
* detecting an object.
|
||||
*
|
||||
* @param position The target position for the gripper, typically in the range [0, 255].
|
||||
* @param speed The speed at which the gripper should move, typically in the range [0, 255].
|
||||
* @param force The force to be applied by the gripper, typically in the range [0, 255].
|
||||
* @return A tuple containing the final position of the gripper as an integer and the object status
|
||||
* as an `ObjectStatus` enumeration value.
|
||||
*
|
||||
* @throws std::runtime_error if there's an error in sending the move command, if the adapter is not
|
||||
* connected to a gripper, or if the gripper does not reach the target position within a
|
||||
* reasonable time frame.
|
||||
*/
|
||||
std::tuple<int, ObjectStatus> move_and_wait_for_pos(int position, int speed, int force);
|
||||
|
||||
private:
|
||||
// Custom deleter for the smart pointer managing the socket descriptor
|
||||
struct SocketDeleter {
|
||||
void operator()(int* ptr) {
|
||||
if (ptr && *ptr >= 0) {
|
||||
close(*ptr);
|
||||
}
|
||||
delete ptr;
|
||||
}
|
||||
};
|
||||
|
||||
// Smart pointer to manage the socket descriptor with the custom deleter
|
||||
std::unique_ptr<int, SocketDeleter> sockfd_;
|
||||
std::mutex socket_mutex_; // Mutex for socket access synchronization
|
||||
|
||||
// bounds of the encoded gripper states
|
||||
int min_position_ = 0;
|
||||
int max_position_ = 255;
|
||||
int min_speed_ = 0;
|
||||
int max_speed_ = 255;
|
||||
int min_force_ = 0;
|
||||
int max_force_ = 255;
|
||||
|
||||
int open_position_ = min_position_;
|
||||
int closed_position_ = max_position_;
|
||||
|
||||
|
||||
/**
|
||||
* Sends a command to the connected Robotiq gripper.
|
||||
*
|
||||
* Constructs and sends a command string to the gripper over the established socket connection.
|
||||
* This method ensures thread-safe access to the socket and handles low-level socket write operations.
|
||||
*
|
||||
* @param cmd The command string to be sent to the gripper. Must conform to the gripper's protocol.
|
||||
* @return True if the command was successfully sent, False if there was an error sending the command.
|
||||
*
|
||||
* @throws std::runtime_error if attempted on a disconnected socket.
|
||||
*/
|
||||
bool sendCommand(const std::string& cmd);
|
||||
|
||||
/**
|
||||
* Receives a response from the Robotiq gripper.
|
||||
*
|
||||
* Waits for and reads a response from the gripper within a specified timeout period. This method
|
||||
* is blocking but allows specifying a timeout to prevent indefinite waiting. It ensures thread-safe
|
||||
* access to the socket and handles low-level socket read operations.
|
||||
*
|
||||
* @param timeout_ms The timeout in milliseconds to wait for a response. Defaults to 2000 milliseconds.
|
||||
* @return A string containing the response from the gripper. If the operation times out or fails, an empty string is returned.
|
||||
*
|
||||
* @throws std::runtime_error if attempted on a disconnected socket or if a select() call fails.
|
||||
*/
|
||||
std::string receiveResponse(int timeout_ms=2000);
|
||||
|
||||
|
||||
/**
|
||||
* Checks if the received data is an acknowledgement message from the gripper.
|
||||
*
|
||||
* This method parses the received data string to determine if it matches the expected acknowledgement
|
||||
* pattern or message, indicating that a previously sent command was received and executed by the gripper.
|
||||
*
|
||||
* @param data The raw string data received from the gripper.
|
||||
* @return True if the data represents an acknowledgement message, False otherwise.
|
||||
*/
|
||||
bool isAck(const std::string& data);
|
||||
|
||||
// Helper Functions
|
||||
/**
|
||||
* Clips a variable to ensure it falls within a specified range.
|
||||
*
|
||||
* This helper function ensures that a given value does not exceed the minimum and maximum bounds
|
||||
* defined for it. It is particularly useful for sanitizing command parameters like position, speed,
|
||||
* and force values before sending them to the gripper.
|
||||
*
|
||||
* @param min_value The minimum allowable value for the variable.
|
||||
* @param variable The current value of the variable to be clipped.
|
||||
* @param max_value The maximum allowable value for the variable.
|
||||
* @return The clipped value, constrained within the specified min and max bounds.
|
||||
*/
|
||||
int clip_val(int min_value, int variable, int max_value);
|
||||
/**
|
||||
* Blocks until the gripper reaches a specified status.
|
||||
*
|
||||
* This method polls the gripper's status at regular intervals until it matches the expected status
|
||||
* or until a timeout occurs. It is used internally to synchronize actions like activation, deactivation,
|
||||
* and calibration with the gripper's state transitions.
|
||||
*
|
||||
* @param expectedStatus The GripperStatus enumeration value the method waits for the gripper to achieve.
|
||||
*
|
||||
* @throws std::runtime_error if the gripper does not reach the expected status within a reasonable time frame,
|
||||
* indicating a possible communication issue or a failure in executing the command.
|
||||
*/
|
||||
void waitForGripperStatus(GripperStatus expectedStatus);
|
||||
|
||||
// Resets the gripper
|
||||
/**
|
||||
* Resets the gripper to its initial state.
|
||||
*
|
||||
* Sends a command to the gripper to transition it to the RESET state. This is often used as a first step
|
||||
* in a sequence of commands that prepare the gripper for operation, ensuring it starts from a known state.
|
||||
*
|
||||
* @note This method can be used to recover the gripper from error states or to prepare it for reactivation.
|
||||
*
|
||||
* @throws std::runtime_error if the adapter is not connected to the gripper or if the reset command fails.
|
||||
*/
|
||||
void reset();
|
||||
|
||||
// Define the command strings and encoding used for communication
|
||||
const std::string SET_COMMAND = "SET";
|
||||
const std::string GET_COMMAND = "GET";
|
||||
const std::string ACK_MESSAGE = "ack";
|
||||
|
||||
// Commands
|
||||
const std::string CMD_ACT = "ACT"; // Activation
|
||||
const std::string CMD_GTO = "GTO"; // GoTo
|
||||
const std::string CMD_ATR = "ATR"; // Automatic Release
|
||||
const std::string CMD_ADR = "ADR"; // Advanced Control
|
||||
const std::string CMD_FOR = "FOR"; // Force
|
||||
const std::string CMD_SPE = "SPE"; // Speed
|
||||
const std::string CMD_POS = "POS"; // Position
|
||||
const std::string CMD_STA = "STA"; // Status
|
||||
const std::string CMD_PRE = "PRE"; // Position Request
|
||||
const std::string CMD_OBJ = "OBJ"; // Object Detection
|
||||
const std::string CMD_FLT = "FLT"; // Fault
|
||||
|
||||
// Constants buffer sizes
|
||||
const size_t BUFFER_SIZE = 1024; // Adjust as necessary
|
||||
const int MAX_RETRIES = 5;
|
||||
const int RETRY_DELAY_MS = 20;
|
||||
};
|
||||
} // end robotiq_interface
|
||||
|
||||
#endif // ROBOTIQ2F_SOCKET_ADAPTER_HPP
|
||||
@ -1,26 +0,0 @@
|
||||
// SocketAdapterBase.hpp
|
||||
|
||||
#ifndef SOCKET_ADAPTER_BASE_HPP
|
||||
#define SOCKET_ADAPTER_BASE_HPP
|
||||
|
||||
#include <string>
|
||||
#include <tuple>
|
||||
|
||||
namespace robotiq_interface
|
||||
{
|
||||
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 robotiq_interface
|
||||
#endif // SOCKET_ADAPTER_BASE_HPP
|
||||
@ -1,117 +0,0 @@
|
||||
// hardware_interface.hpp
|
||||
|
||||
#ifndef ROBOTIQ2F_HARDWARE_INTERFACE_HPP
|
||||
#define ROBOTIQ2F_HARDWARE_INTERFACE_HPP
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <atomic>
|
||||
|
||||
#include "robotiq_2f_interface/visibility_control.hpp"
|
||||
|
||||
#include "robotiq_2f_interface/SocketAdapterBase.hpp"
|
||||
#include "robotiq_2f_interface/Robotiq2fSocketAdapter.hpp"
|
||||
#include "robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp"
|
||||
|
||||
#include <hardware_interface/handle.hpp>
|
||||
#include <hardware_interface/hardware_info.hpp>
|
||||
#include <hardware_interface/system_interface.hpp>
|
||||
#include <hardware_interface/types/hardware_interface_type_values.hpp>
|
||||
#include <hardware_interface/types/hardware_interface_return_values.hpp>
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp/clock.hpp>
|
||||
|
||||
#include <stdexcept>
|
||||
|
||||
namespace robotiq_interface
|
||||
{
|
||||
class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterface
|
||||
{
|
||||
public:
|
||||
// Constructors
|
||||
ROBOTIQ_INTERFACE_PUBLIC RobotiqGripperHardwareInterface();
|
||||
ROBOTIQ_INTERFACE_PUBLIC ~RobotiqGripperHardwareInterface();
|
||||
// ROBOTIQ_DRIVER_PUBLIC explicit RobotiqGripperHardwareInterface(std::unique_ptr<SocketFactory> socket_factory);
|
||||
|
||||
// Lifecycle Management
|
||||
ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override;
|
||||
ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
|
||||
ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
|
||||
ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
|
||||
|
||||
// State and Command Interfaces
|
||||
ROBOTIQ_INTERFACE_PUBLIC std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
|
||||
ROBOTIQ_INTERFACE_PUBLIC std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
|
||||
|
||||
// Data Access (Read/Write)
|
||||
ROBOTIQ_INTERFACE_PUBLIC hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override;
|
||||
ROBOTIQ_INTERFACE_PUBLIC hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;
|
||||
|
||||
protected: // Likely changes the access to protected from private
|
||||
// Members for driver interaction
|
||||
bool use_mock_hardware_;
|
||||
std::unique_ptr<SocketAdapterBase> socket_adapter_;
|
||||
void configureAdapter(bool useMock);
|
||||
|
||||
// Background communication thread
|
||||
std::thread communication_thread_;
|
||||
std::atomic<bool> communication_thread_is_running_;
|
||||
std::mutex resource_mutex_;
|
||||
void background_task();
|
||||
|
||||
static constexpr double NO_NEW_CMD_ = std::numeric_limits<double>::quiet_NaN();
|
||||
|
||||
// Conversion Methods
|
||||
double convertRawToPosition(int raw_position);
|
||||
int convertPositionToRaw(double position);
|
||||
int convertSpeedToRaw(double speed);
|
||||
int convertForceToRaw(double force);
|
||||
|
||||
// Gripper Commands
|
||||
std::atomic<uint8_t> write_command_;
|
||||
std::atomic<uint8_t> write_force_;
|
||||
std::atomic<uint8_t> write_speed_;
|
||||
std::atomic<uint8_t> gripper_current_state_;
|
||||
|
||||
|
||||
// Gripper State Tracking
|
||||
double gripper_position_ = 0.0;
|
||||
double gripper_velocity_ = 0.0;
|
||||
double gripper_position_command_ = 0.0;
|
||||
|
||||
rclcpp::Clock::SharedPtr clock_ = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
|
||||
rclcpp::Time last_update_time_ = clock_ -> now();
|
||||
rclcpp::Time current_time_ = clock_ -> now();
|
||||
|
||||
double reactivate_gripper_cmd_ = 0.0;
|
||||
std::atomic<bool> reactivate_gripper_async_cmd_;
|
||||
double reactivate_gripper_response_ = 0.0;
|
||||
double gripper_force_ = 0.0;
|
||||
double gripper_speed_ = 0.0;
|
||||
std::atomic<std::optional<bool>> reactivate_gripper_async_response_;
|
||||
|
||||
// Connection Variables
|
||||
std::string robot_ip_;
|
||||
int robot_port_;
|
||||
|
||||
// Parameters for physical limits and configuration
|
||||
double min_position_;
|
||||
double max_position_;
|
||||
double min_speed_;
|
||||
double max_speed_;
|
||||
double min_force_;
|
||||
double max_force_;
|
||||
|
||||
// loop time
|
||||
const std::chrono::milliseconds gripperCommsLoopPeriod = std::chrono::milliseconds{ 10 };
|
||||
|
||||
// Logger
|
||||
const rclcpp::Logger logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface");
|
||||
};
|
||||
} // end robotiq_interface
|
||||
|
||||
#endif //ROBOTIQ2F_HARDWARE_INTERFACE_HPP
|
||||
@ -1,56 +0,0 @@
|
||||
// Copyright 2017 Open Source Robotics Foundation, Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/* This header must be included by all rclcpp headers which declare symbols
|
||||
* which are defined in the rclcpp library. When not building the rclcpp
|
||||
* library, i.e. when using the headers in other package's code, the contents
|
||||
* of this header change the visibility of certain symbols which the rclcpp
|
||||
* library cannot have, but the consuming code must have inorder to link.
|
||||
*/
|
||||
|
||||
#ifndef ROBOTIQ_INTERFACE__VISIBILITY_CONTROL_HPP_
|
||||
#define ROBOTIQ_INTERFACE__VISIBILITY_CONTROL_HPP_
|
||||
|
||||
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
|
||||
// https://gcc.gnu.org/wiki/Visibility
|
||||
|
||||
#if defined _WIN32 || defined __CYGWIN__
|
||||
#ifdef __GNUC__
|
||||
#define ROBOTIQ_INTERFACE_EXPORT __attribute__((dllexport))
|
||||
#define ROBOTIQ_INTERFACE_IMPORT __attribute__((dllimport))
|
||||
#else
|
||||
#define ROBOTIQ_INTERFACE_EXPORT __declspec(dllexport)
|
||||
#define ROBOTIQ_INTERFACE_IMPORT __declspec(dllimport)
|
||||
#endif
|
||||
#ifdef ROBOTIQ_INTERFACE_BUILDING_DLL
|
||||
#define ROBOTIQ_INTERFACE_PUBLIC ROBOTIQ_INTERFACE_EXPORT
|
||||
#else
|
||||
#define ROBOTIQ_INTERFACE_PUBLIC ROBOTIQ_INTERFACE_IMPORT
|
||||
#endif
|
||||
#define ROBOTIQ_INTERFACE_PUBLIC_TYPE ROBOTIQ_INTERFACE_PUBLIC
|
||||
#define ROBOTIQ_INTERFACE_LOCAL
|
||||
#else
|
||||
#define ROBOTIQ_INTERFACE_EXPORT __attribute__((visibility("default")))
|
||||
#define ROBOTIQ_INTERFACE_IMPORT
|
||||
#if __GNUC__ >= 4
|
||||
#define ROBOTIQ_INTERFACE_PUBLIC __attribute__((visibility("default")))
|
||||
#define ROBOTIQ_INTERFACE_LOCAL __attribute__((visibility("hidden")))
|
||||
#else
|
||||
#define ROBOTIQ_INTERFACE_PUBLIC
|
||||
#define ROBOTIQ_INTERFACE_LOCAL
|
||||
#endif
|
||||
#define ROBOTIQ_INTERFACE_PUBLIC_TYPE
|
||||
#endif
|
||||
|
||||
#endif // ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_
|
||||
@ -1,372 +0,0 @@
|
||||
#include "robotiq_2f_interface/Robotiq2fSocketAdapter.hpp"
|
||||
|
||||
|
||||
namespace robotiq_interface
|
||||
{
|
||||
Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : sockfd_(new int(-1)) {
|
||||
}
|
||||
|
||||
|
||||
Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() {
|
||||
disconnect(); // Ensure the socket is closed properly.
|
||||
}
|
||||
|
||||
// Connection and disconnection
|
||||
bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
|
||||
std::lock_guard<std::mutex> lock(socket_mutex_);
|
||||
if (*sockfd_ >= 0) {
|
||||
std::cerr << "Already connected. Disconnecting to reconnect.\n";
|
||||
disconnect();
|
||||
}
|
||||
|
||||
int sock = socket(AF_INET, SOCK_STREAM, 0);
|
||||
if (sock < 0) {
|
||||
std::cerr << "Socket creation failed: " << strerror(errno) << "\n";
|
||||
return false;
|
||||
}
|
||||
*sockfd_ = sock;
|
||||
|
||||
sockaddr_in serv_addr{};
|
||||
memset(&serv_addr, 0, sizeof(serv_addr));
|
||||
serv_addr.sin_family = AF_INET;
|
||||
serv_addr.sin_port = htons(port);
|
||||
|
||||
if (inet_pton(AF_INET, hostname.c_str(), &serv_addr.sin_addr) <= 0) {
|
||||
std::cerr << "Invalid address: " << hostname << "\n";
|
||||
close(*sockfd_);
|
||||
*sockfd_ = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (::connect(*sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) {
|
||||
std::cerr << "Connection to " << hostname << ":" << port << " failed: " << strerror(errno) << "\n";
|
||||
close(*sockfd_);
|
||||
*sockfd_ = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Robotiq2fSocketAdapter::disconnect() {
|
||||
std::lock_guard<std::mutex> lock(socket_mutex_);
|
||||
if (*sockfd_ >= 0) {
|
||||
if (close(*sockfd_) == -1) {
|
||||
std::cerr << "Error closing socket: " << strerror(errno) << "\n";
|
||||
} else {
|
||||
std::cout << "Disconnected successfully.\n";
|
||||
}
|
||||
*sockfd_ = -1;
|
||||
}
|
||||
}
|
||||
|
||||
// Utility methods
|
||||
bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
|
||||
std::lock_guard<std::mutex> lock(socket_mutex_);
|
||||
if (*sockfd_ < 0) {
|
||||
std::cerr << "Attempted to send command on a disconnected socket.\n";
|
||||
return false;
|
||||
}
|
||||
|
||||
ssize_t result = send(*sockfd_, cmd.c_str(), cmd.length(), 0);
|
||||
if (result < 0) {
|
||||
std::cerr << "Failed to send command: " << strerror(errno) << "\n";
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms) {
|
||||
std::lock_guard<std::mutex> lock(socket_mutex_);
|
||||
if (*sockfd_ < 0) {
|
||||
std::cerr << "Attempted to receive response on a disconnected socket.\n";
|
||||
return "";
|
||||
}
|
||||
|
||||
std::string result;
|
||||
char buffer[BUFFER_SIZE];
|
||||
bool complete_response = false;
|
||||
|
||||
timeval tv;
|
||||
tv.tv_sec = timeout_ms / 1000;
|
||||
tv.tv_usec = (timeout_ms % 1000) * 1000;
|
||||
|
||||
// Loop to receive data until a complete response is obtained or a timeout occurs
|
||||
while (!complete_response) {
|
||||
fd_set readfds;
|
||||
FD_ZERO(&readfds);
|
||||
FD_SET(*sockfd_, &readfds);
|
||||
|
||||
int activity = select(*sockfd_ + 1, &readfds, nullptr, nullptr, &tv);
|
||||
if (activity == -1) {
|
||||
std::cerr << "[receiveResponse] Error in select(): " << strerror(errno) << "\n";
|
||||
return "";
|
||||
} else if (activity == 0) {
|
||||
std::cerr << "Timeout occurred while waiting for response.\n";
|
||||
return "";
|
||||
}
|
||||
|
||||
ssize_t bytes_read = recv(*sockfd_, buffer, BUFFER_SIZE - 1, 0);
|
||||
if (bytes_read < 0) {
|
||||
if (errno != EAGAIN && errno != EWOULDBLOCK) {
|
||||
std::cerr << "Failed to receive response: " << strerror(errno) << "\n";
|
||||
}
|
||||
return "";
|
||||
} else if (bytes_read == 0) {
|
||||
std::cerr << "Connection closed by peer.\n";
|
||||
return "";
|
||||
} else {
|
||||
buffer[bytes_read] = '\0';
|
||||
result += buffer;
|
||||
|
||||
// Check if we have a complete response:
|
||||
if (result.find("\n") != std::string::npos) {
|
||||
complete_response = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
bool Robotiq2fSocketAdapter::isAck(const std::string& data) {
|
||||
return data == ACK_MESSAGE;
|
||||
}
|
||||
|
||||
int Robotiq2fSocketAdapter::clip_val(int min_value, int value, int max_value) {
|
||||
return std::max(min_value, std::min(value, max_value));
|
||||
}
|
||||
|
||||
void Robotiq2fSocketAdapter::waitForGripperStatus(GripperStatus expectedStatus) {
|
||||
int retries = 0;
|
||||
int status = getGripperVariable(CMD_STA);
|
||||
while (static_cast<GripperStatus>(status) != expectedStatus && retries < MAX_RETRIES) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(RETRY_DELAY_MS));
|
||||
status = getGripperVariable(CMD_STA);
|
||||
retries++;
|
||||
}
|
||||
if (retries >= MAX_RETRIES) {
|
||||
throw std::runtime_error("Gripper failed to reach expected status.");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Gripper variable Getter and Setter
|
||||
bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, int>& variableMap) {
|
||||
return setGripperVariables(reinterpret_cast<const std::map<std::string, double>&>(variableMap));
|
||||
}
|
||||
|
||||
bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, double>& variableMap) {
|
||||
// Build the command string
|
||||
std::string cmd = SET_COMMAND;
|
||||
for (const auto& [variable, value] : variableMap) {
|
||||
std::stringstream ss;
|
||||
ss << value;
|
||||
cmd += " " + variable + " " + ss.str();
|
||||
}
|
||||
cmd += "\n";
|
||||
|
||||
// Send the command and receive the response
|
||||
std::lock_guard<std::mutex> lock(socket_mutex_);
|
||||
if (*sockfd_ < 0) {
|
||||
throw std::runtime_error("Cannot set variables on a disconnected socket.");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!sendCommand(cmd)) {
|
||||
throw std::runtime_error("[setGripperVariables] Sending command failed.");
|
||||
return false;
|
||||
}
|
||||
|
||||
std::string response = receiveResponse();
|
||||
return isAck(response);
|
||||
}
|
||||
|
||||
bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, int value) {
|
||||
return setGripperVariable(variable, static_cast<double>(value)); // Convert int to double
|
||||
}
|
||||
|
||||
bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, double value) {
|
||||
std::stringstream ss;
|
||||
ss << value; // Convert the value to a string
|
||||
|
||||
std::string cmd = SET_COMMAND + " " + variable + " " + ss.str() + "\n";
|
||||
|
||||
std::lock_guard<std::mutex> lock(socket_mutex_);
|
||||
if (*sockfd_ < 0) {
|
||||
throw std::runtime_error("Cannot set variables on a disconnected socket.");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!sendCommand(cmd)) {
|
||||
throw std::runtime_error("[setGripperVariable] Sending command failed.");
|
||||
return false; // Sending command failed
|
||||
}
|
||||
|
||||
std::string response = receiveResponse();
|
||||
return isAck(response);
|
||||
}
|
||||
|
||||
int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
|
||||
std::string cmd = GET_COMMAND + " " + variable + "\n";
|
||||
|
||||
std::lock_guard<std::mutex> lock(socket_mutex_);
|
||||
if (*sockfd_ < 0) {
|
||||
throw std::runtime_error("Cannot get variables on a disconnected socket.");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (!sendCommand(cmd)) {
|
||||
throw std::runtime_error("[getGripperVariable] Sending command failed.");
|
||||
return -1; // Error sending command
|
||||
}
|
||||
|
||||
std::string response = receiveResponse();
|
||||
|
||||
// Parse the response (assuming format: "variable_name value")
|
||||
std::istringstream iss(response);
|
||||
std::string var_name, value_str;
|
||||
|
||||
if (!(iss >> var_name >> value_str)) {
|
||||
throw std::runtime_error("Invalid gripper response format.");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (var_name != variable) {
|
||||
throw std::runtime_error("Unexpected response - variable name mismatch.");
|
||||
return -1;
|
||||
}
|
||||
|
||||
try {
|
||||
return std::stoi(value_str);
|
||||
} catch (const std::exception& ex) {
|
||||
std::cerr << "Failed to parse gripper response: " << ex.what() << std::endl;
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
// Activation, deactivation and reset of the Gripper
|
||||
void Robotiq2fSocketAdapter::activate(bool autoCalibrate) {
|
||||
reset(); // Always start with a reset to ensure a known state
|
||||
setGripperVariable(CMD_ACT, 1); // Activate the gripper
|
||||
waitForGripperStatus(GripperStatus::ACTIVE); // Wait until active
|
||||
|
||||
if (autoCalibrate) {
|
||||
auto_calibrate();
|
||||
}
|
||||
}
|
||||
|
||||
void Robotiq2fSocketAdapter::reset() {
|
||||
setGripperVariable(CMD_ACT, 0);
|
||||
setGripperVariable(CMD_ATR, 0);
|
||||
waitForGripperStatus(GripperStatus::RESET);
|
||||
}
|
||||
|
||||
|
||||
void Robotiq2fSocketAdapter::deactivate() {
|
||||
if (is_active()) {
|
||||
reset();
|
||||
}
|
||||
}
|
||||
|
||||
// State subsscriber
|
||||
bool Robotiq2fSocketAdapter::is_active(){
|
||||
GripperStatus status = static_cast<GripperStatus>(getGripperVariable(CMD_STA));
|
||||
return GripperStatus::ACTIVE == status;
|
||||
}
|
||||
|
||||
int Robotiq2fSocketAdapter::force(){
|
||||
return getGripperVariable(CMD_FOR);
|
||||
}
|
||||
|
||||
int Robotiq2fSocketAdapter::speed(){
|
||||
return getGripperVariable(CMD_SPE);
|
||||
}
|
||||
|
||||
int Robotiq2fSocketAdapter::position(){
|
||||
return getGripperVariable(CMD_POS);
|
||||
}
|
||||
|
||||
// Movement Methods
|
||||
std::tuple<bool, int> Robotiq2fSocketAdapter::move(int position, int speed, int force) {
|
||||
int clippedPosition = clip_val(min_position_, position, max_position_);
|
||||
int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
|
||||
int clippedForce = clip_val(min_force_, force, max_force_);
|
||||
|
||||
// Create a map for gripper variables (similar to Python's OrderedDict)
|
||||
std::map<std::string, int> variableMap = {
|
||||
{ CMD_POS, clippedPosition },
|
||||
{ CMD_SPE, clippedSpeed },
|
||||
{ CMD_FOR, clippedForce },
|
||||
{ CMD_GTO, 1 }
|
||||
};
|
||||
|
||||
bool setResult = setGripperVariables(variableMap);
|
||||
return std::make_tuple(setResult, clippedPosition);
|
||||
}
|
||||
|
||||
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 clippedSpeed = clip_val(min_speed_, speed, max_speed_);
|
||||
int clippedForce = clip_val(min_force_, force, max_force_);
|
||||
|
||||
// Set gripper variables to initiate the move
|
||||
std::map<std::string, int> variableMap = {
|
||||
{ CMD_POS, clippedPosition },
|
||||
{ CMD_SPE, clippedSpeed },
|
||||
{ CMD_FOR, clippedForce },
|
||||
{ CMD_GTO, 1 }
|
||||
};
|
||||
bool setResult = setGripperVariables(variableMap);
|
||||
if (!setResult) {
|
||||
// Handle the error case, e.g., throw an exception
|
||||
throw std::runtime_error("Failed to set variables for move.");
|
||||
}
|
||||
|
||||
// Wait until position is acknowledged:
|
||||
while(getGripperVariable(CMD_PRE) != clippedPosition) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10)); // Adjust sleep as needed
|
||||
}
|
||||
|
||||
// Wait until not moving
|
||||
int currentObjectStatus = getGripperVariable(CMD_OBJ);
|
||||
while (ObjectStatus(currentObjectStatus) == ObjectStatus::MOVING) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10)); // Adjust sleep as needed
|
||||
currentObjectStatus = getGripperVariable(CMD_OBJ);
|
||||
}
|
||||
|
||||
// Report final position and object status
|
||||
int finalPosition = getGripperVariable(CMD_POS);
|
||||
ObjectStatus finalStatus = static_cast<ObjectStatus>(currentObjectStatus);
|
||||
return std::make_tuple(finalPosition, finalStatus);
|
||||
}
|
||||
|
||||
|
||||
void Robotiq2fSocketAdapter::auto_calibrate(bool log) {
|
||||
// Open in case we are holding an object
|
||||
std::tuple<int, ObjectStatus> result = move_and_wait_for_pos(open_position_, 64, 1);
|
||||
if (std::get<1>(result) != ObjectStatus::AT_DEST) {
|
||||
throw std::runtime_error("Calibration failed opening to start.");
|
||||
}
|
||||
|
||||
// Close as far as possible
|
||||
result = move_and_wait_for_pos(closed_position_, 64, 1);
|
||||
if (std::get<1>(result) != ObjectStatus::AT_DEST) {
|
||||
throw std::runtime_error("Calibration failed because of an object.");
|
||||
}
|
||||
max_position_ = std::get<0>(result); // Update max position
|
||||
|
||||
// Open as far as possible
|
||||
result = move_and_wait_for_pos(open_position_, 64, 1);
|
||||
if (std::get<1>(result) != ObjectStatus::AT_DEST) {
|
||||
throw std::runtime_error("Calibration failed because of an object.");
|
||||
}
|
||||
min_position_ = std::get<0>(result); // Update min position
|
||||
|
||||
if (log) {
|
||||
std::cout << "Gripper auto-calibrated to ["
|
||||
<< min_position_ << ", " << max_position_ << "]\n";
|
||||
}
|
||||
}
|
||||
} // end robotiq_interface
|
||||
@ -1,469 +0,0 @@
|
||||
#include "robotiq_2f_interface/hardware_interface.hpp"
|
||||
|
||||
namespace robotiq_interface
|
||||
{
|
||||
|
||||
const std::string robot_ip_parameter_name = "robot_ip";
|
||||
const std::string robot_ip_default = "192.168.1.1";
|
||||
|
||||
const std::string robot_port_parameter_name = "robot_port";
|
||||
const int robot_port_default = 63352;
|
||||
|
||||
const std::string use_mock_hardware_parameter_name = "use_fake_hardware";
|
||||
const bool use_mock_hardware_default = true;
|
||||
|
||||
const std::string min_position_parameter_name = "min_position";
|
||||
const double min_position_default = 0.01;
|
||||
|
||||
const std::string max_position_parameter_name = "max_position";
|
||||
const double max_position_default = 0.0;
|
||||
|
||||
const std::string min_speed_parameter_name = "min_speed";
|
||||
const double min_speed_default = 0.0;
|
||||
|
||||
const std::string max_speed_parameter_name = "max_speed";
|
||||
const double max_speed_default = 0.0;
|
||||
|
||||
const std::string min_force_parameter_name = "min_force";
|
||||
const double min_force_default = 0.0;
|
||||
|
||||
const std::string max_force_parameter_name = "max_force";
|
||||
const double max_force_default = 0.0;
|
||||
|
||||
RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface()
|
||||
: communication_thread_is_running_(false)
|
||||
{
|
||||
RCLCPP_INFO(logger_, "Constructor");
|
||||
}
|
||||
|
||||
RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface()
|
||||
{
|
||||
if (communication_thread_is_running_.load())
|
||||
{
|
||||
communication_thread_is_running_.store(false);
|
||||
if (communication_thread_.joinable())
|
||||
{
|
||||
communication_thread_.join();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info)
|
||||
{
|
||||
RCLCPP_DEBUG(logger_, "on_init");
|
||||
|
||||
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
|
||||
{
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
if (info.hardware_parameters.count(robot_ip_parameter_name) > 0) {
|
||||
robot_ip_ = info.hardware_parameters.at(robot_ip_parameter_name);
|
||||
} else {
|
||||
robot_ip_ = robot_ip_default;
|
||||
}
|
||||
RCLCPP_INFO(logger_, "Accessing parameter: Robot IP, Value: %s", robot_ip_.c_str());
|
||||
|
||||
if (info.hardware_parameters.count(robot_port_parameter_name) > 0) {
|
||||
robot_port_ = std::stoi(info.hardware_parameters.at(robot_port_parameter_name));
|
||||
} else {
|
||||
robot_port_ = robot_port_default;
|
||||
}
|
||||
RCLCPP_INFO(logger_, "Accessing parameter: Robot Port, Value: %d", robot_port_);
|
||||
|
||||
std::string param_value;
|
||||
if (info.hardware_parameters.count(use_mock_hardware_parameter_name) > 0) {
|
||||
param_value = info.hardware_parameters.at(use_mock_hardware_parameter_name);
|
||||
use_mock_hardware_ = (param_value == "true" || param_value == "True");
|
||||
} else {
|
||||
use_mock_hardware_ = use_mock_hardware_default;
|
||||
}
|
||||
RCLCPP_INFO(logger_, "Accessing parameter: use fake hardware , Value: %s", param_value.c_str());
|
||||
|
||||
if (info.hardware_parameters.count(min_position_parameter_name) > 0) {
|
||||
min_position_ = std::stod(info.hardware_parameters.at(min_position_parameter_name));
|
||||
} else {
|
||||
min_position_ = min_position_default;
|
||||
}
|
||||
RCLCPP_INFO(logger_, "Accessing parameter: min position, Value: %f", min_position_);
|
||||
|
||||
if (info.hardware_parameters.count(max_position_parameter_name) > 0) {
|
||||
max_position_ = std::stod(info.hardware_parameters.at(max_position_parameter_name));
|
||||
} else {
|
||||
max_position_ = max_position_default;
|
||||
}
|
||||
RCLCPP_INFO(logger_, "Accessing parameter: max position, Value: %f", max_position_);
|
||||
|
||||
if (info.hardware_parameters.count(min_speed_parameter_name) > 0) {
|
||||
min_speed_ = std::stod(info.hardware_parameters.at(min_speed_parameter_name));
|
||||
} else {
|
||||
min_speed_ = min_speed_default;
|
||||
}
|
||||
RCLCPP_INFO(logger_, "Accessing parameter: min speed, Value: %f", min_speed_);
|
||||
|
||||
if (info.hardware_parameters.count(max_speed_parameter_name) > 0) {
|
||||
max_speed_ = std::stod(info.hardware_parameters.at(max_speed_parameter_name));
|
||||
} else {
|
||||
max_speed_ = max_speed_default;
|
||||
}
|
||||
RCLCPP_INFO(logger_, "Accessing parameter: max speed, Value: %f", max_speed_);
|
||||
|
||||
if (info.hardware_parameters.count(min_force_parameter_name) > 0) {
|
||||
min_force_ = std::stod(info.hardware_parameters.at(min_force_parameter_name));
|
||||
} else {
|
||||
min_force_ = robot_port_default;
|
||||
}
|
||||
RCLCPP_INFO(logger_, "Accessing parameter: min force, Value: %f", min_force_);
|
||||
|
||||
if (info.hardware_parameters.count(max_force_parameter_name) > 0) {
|
||||
max_force_ = std::stod(info.hardware_parameters.at(max_force_parameter_name));
|
||||
} else {
|
||||
max_force_ = max_force_default;
|
||||
}
|
||||
RCLCPP_INFO(logger_, "Accessing parameter: max force, Value: %f", max_force_);
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
RCLCPP_FATAL(logger_, "Failed to load parameters: %s", e.what());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
gripper_position_ = std::numeric_limits<double>::quiet_NaN();
|
||||
gripper_velocity_ = std::numeric_limits<double>::quiet_NaN();
|
||||
gripper_position_command_ = std::numeric_limits<double>::quiet_NaN();
|
||||
reactivate_gripper_cmd_ = NO_NEW_CMD_;
|
||||
reactivate_gripper_async_cmd_.store(false);
|
||||
|
||||
const hardware_interface::ComponentInfo& joint = info_.joints[0];
|
||||
|
||||
// There is one command interface: position.
|
||||
if (joint.command_interfaces.size() != 1)
|
||||
{
|
||||
RCLCPP_FATAL(logger_, "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(),
|
||||
joint.command_interfaces.size());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
|
||||
{
|
||||
RCLCPP_FATAL(logger_, "Joint '%s' has %s command interfaces found. '%s' expected.", joint.name.c_str(),
|
||||
joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
// There are two state interfaces: position and velocity.
|
||||
if (joint.state_interfaces.size() != 2)
|
||||
{
|
||||
RCLCPP_FATAL(logger_, "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(),
|
||||
joint.state_interfaces.size());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 2; ++i)
|
||||
{
|
||||
if (!(joint.state_interfaces[i].name == hardware_interface::HW_IF_POSITION ||
|
||||
joint.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY))
|
||||
{
|
||||
RCLCPP_FATAL(logger_, "Joint '%s' has %s state interface. Expected %s or %s.", joint.name.c_str(),
|
||||
joint.state_interfaces[i].name.c_str(), hardware_interface::HW_IF_POSITION,
|
||||
hardware_interface::HW_IF_VELOCITY);
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State& previous_state)
|
||||
{
|
||||
RCLCPP_DEBUG(logger_, "on_configure");
|
||||
try
|
||||
{
|
||||
if (hardware_interface::SystemInterface::on_configure(previous_state) != CallbackReturn::SUCCESS)
|
||||
{
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
configureAdapter(use_mock_hardware_);
|
||||
if (!socket_adapter_->connect(robot_ip_, robot_port_))
|
||||
{
|
||||
RCLCPP_ERROR(logger_, "Cannot connect to the Robotiq gripper at %s:%d", robot_ip_.c_str(), robot_port_);
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
RCLCPP_ERROR(logger_, "Cannot configure the Robotiq gripper: %s", e.what());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
|
||||
{
|
||||
RCLCPP_DEBUG(logger_, "on_activate");
|
||||
|
||||
// set some default values for joints
|
||||
if (std::isnan(gripper_position_))
|
||||
{
|
||||
gripper_position_ = 0;
|
||||
gripper_velocity_ = 0;
|
||||
gripper_position_command_ = 0;
|
||||
}
|
||||
|
||||
// Activate the gripper.
|
||||
try
|
||||
{
|
||||
socket_adapter_->deactivate();
|
||||
socket_adapter_->activate();
|
||||
|
||||
communication_thread_is_running_.store(true);
|
||||
communication_thread_ = std::thread([this] { this->background_task(); });
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
RCLCPP_FATAL(logger_, "Failed to communicate with the Robotiq gripper: %s", e.what());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(logger_, "Robotiq Gripper successfully activated!");
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
|
||||
{
|
||||
RCLCPP_DEBUG(logger_, "on_deactivate");
|
||||
|
||||
communication_thread_is_running_.store(false);
|
||||
communication_thread_.join();
|
||||
if (communication_thread_.joinable())
|
||||
{
|
||||
communication_thread_.join();
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
socket_adapter_->deactivate();
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
RCLCPP_ERROR(logger_, "Failed to deactivate the Robotiq gripper: %s", e.what());
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
RCLCPP_INFO(logger_, "Robotiq Gripper successfully deactivated!");
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
std::vector<hardware_interface::StateInterface> RobotiqGripperHardwareInterface::export_state_interfaces()
|
||||
{
|
||||
RCLCPP_DEBUG(logger_, "export_state_interfaces");
|
||||
|
||||
std::vector<hardware_interface::StateInterface> state_interfaces;
|
||||
|
||||
state_interfaces.emplace_back(
|
||||
hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_));
|
||||
state_interfaces.emplace_back(
|
||||
hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_));
|
||||
|
||||
for (const auto& interface : state_interfaces) {
|
||||
RCLCPP_DEBUG(logger_, "Exporting state interface for joint: %s type: %s",
|
||||
interface.get_name().c_str(), interface.get_interface_name().c_str());
|
||||
}
|
||||
|
||||
return state_interfaces;
|
||||
}
|
||||
|
||||
std::vector<hardware_interface::CommandInterface> RobotiqGripperHardwareInterface::export_command_interfaces()
|
||||
{
|
||||
RCLCPP_DEBUG(logger_, "export_command_interfaces");
|
||||
|
||||
std::vector<hardware_interface::CommandInterface> command_interfaces;
|
||||
|
||||
command_interfaces.emplace_back(
|
||||
hardware_interface::CommandInterface(
|
||||
info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_command_
|
||||
)
|
||||
);
|
||||
|
||||
// Handling potential uninitialized parameter 'gripper_speed_multiplier'
|
||||
double gripper_speed_multiplier = 1.0;
|
||||
if (info_.hardware_parameters.find("gripper_speed_multiplier") != info_.hardware_parameters.end()) {
|
||||
gripper_speed_multiplier = std::stod(info_.hardware_parameters["gripper_speed_multiplier"]);
|
||||
}
|
||||
gripper_speed_ = gripper_speed_multiplier;
|
||||
|
||||
// Handling potential uninitialized parameter 'gripper_force_multiplier'
|
||||
double gripper_force_multiplier = 1.0;
|
||||
if (info_.hardware_parameters.find("gripper_force_multiplier") != info_.hardware_parameters.end()) {
|
||||
gripper_force_multiplier = std::stod(info_.hardware_parameters["gripper_force_multiplier"]);
|
||||
}
|
||||
gripper_force_ = gripper_force_multiplier;
|
||||
|
||||
command_interfaces.emplace_back(
|
||||
hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_velocity", &gripper_speed_));
|
||||
command_interfaces.emplace_back(
|
||||
hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_effort", &gripper_force_));
|
||||
command_interfaces.emplace_back(
|
||||
hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_cmd", &reactivate_gripper_cmd_));
|
||||
command_interfaces.emplace_back(
|
||||
hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_response", &reactivate_gripper_response_));
|
||||
|
||||
for (const auto& interface : command_interfaces) {
|
||||
RCLCPP_DEBUG(logger_, "Exporting command interface for joint: %s type: %s",
|
||||
interface.get_name().c_str(), interface.get_interface_name().c_str());
|
||||
}
|
||||
|
||||
return command_interfaces;
|
||||
}
|
||||
|
||||
hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
|
||||
{
|
||||
RCLCPP_DEBUG(logger_, "Reading state from the gripper");
|
||||
|
||||
// Ensure that the node uses the appropriate clock source
|
||||
|
||||
current_time_ = clock_->now();
|
||||
|
||||
try {
|
||||
int raw_position = socket_adapter_->position();
|
||||
double new_position = convertRawToPosition(raw_position);
|
||||
|
||||
if (!std::isnan(gripper_position_)) {
|
||||
double time_difference = (current_time_ - last_update_time_).seconds();
|
||||
if (time_difference > 0) {
|
||||
gripper_velocity_ = (new_position - gripper_position_) / time_difference;
|
||||
}
|
||||
}
|
||||
|
||||
gripper_position_ = new_position;
|
||||
last_update_time_ = current_time_;
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(logger_, "Failed to read from Robotiq gripper: %s", e.what());
|
||||
return hardware_interface::return_type::ERROR;
|
||||
}
|
||||
|
||||
// Process asynchronous reactivation response if available
|
||||
if (reactivate_gripper_async_response_.load().has_value()) {
|
||||
reactivate_gripper_response_ = reactivate_gripper_async_response_.load().value();
|
||||
reactivate_gripper_async_response_.store(std::nullopt); // Reset response
|
||||
}
|
||||
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
hardware_interface::return_type RobotiqGripperHardwareInterface::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
|
||||
{
|
||||
try {
|
||||
// Use the conversion methods to scale the command values to the range expected by the hardware
|
||||
int scaled_position = convertPositionToRaw(gripper_position_command_);
|
||||
int scaled_speed = convertSpeedToRaw(gripper_speed_);
|
||||
int scaled_force = convertForceToRaw(gripper_force_);
|
||||
|
||||
// Store the scaled values into atomic variables
|
||||
write_command_.store(scaled_position);
|
||||
write_speed_.store(scaled_speed);
|
||||
write_force_.store(scaled_force);
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(logger_, "Failed to write to Robotiq gripper: %s", e.what());
|
||||
return hardware_interface::return_type::ERROR;
|
||||
}
|
||||
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
void RobotiqGripperHardwareInterface::background_task()
|
||||
{
|
||||
while (communication_thread_is_running_.load())
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(resource_mutex_);
|
||||
try
|
||||
{
|
||||
// Re-activate the gripper
|
||||
// This can be used, for example, to re-run the auto-calibration.
|
||||
if (reactivate_gripper_async_cmd_.load())
|
||||
{
|
||||
socket_adapter_->deactivate();
|
||||
socket_adapter_->activate();
|
||||
reactivate_gripper_async_cmd_.store(false);
|
||||
reactivate_gripper_async_response_.store(true);
|
||||
}
|
||||
|
||||
// Write the latest command to the gripper.
|
||||
int scaled_position = write_command_.load();
|
||||
int scaled_speed = write_speed_.load();
|
||||
int scaled_force = write_force_.load();
|
||||
|
||||
auto result = socket_adapter_->move(scaled_position, scaled_speed, scaled_force);
|
||||
if (!std::get<0>(result)) {
|
||||
throw std::runtime_error("Failed to send move command to Robotiq gripper.");
|
||||
}
|
||||
|
||||
// Read the state of the gripper.
|
||||
int raw_position = socket_adapter_->position();
|
||||
gripper_current_state_.store(convertRawToPosition(raw_position));
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
RCLCPP_ERROR(logger_, "Error in background task: %s", e.what());
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(gripperCommsLoopPeriod); // Sleep for a predefined period
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 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
|
||||
double RobotiqGripperHardwareInterface::convertRawToPosition(int raw_position) {
|
||||
if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) {
|
||||
throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly.");
|
||||
}
|
||||
double scaled_position = min_position_ + (static_cast<double>(raw_position) / 255.0) * (max_position_ - min_position_);
|
||||
return std::max(min_position_, std::min(scaled_position, max_position_));
|
||||
}
|
||||
|
||||
int RobotiqGripperHardwareInterface::convertPositionToRaw(double position) {
|
||||
if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) {
|
||||
throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly.");
|
||||
}
|
||||
double scaled = (position - min_position_) / (max_position_ - min_position_) * 255.0;
|
||||
return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
|
||||
}
|
||||
|
||||
int RobotiqGripperHardwareInterface::convertSpeedToRaw(double speed) {
|
||||
if (std::isnan(min_speed_) || std::isnan(max_speed_) || min_speed_ >= max_speed_) {
|
||||
throw std::runtime_error("Invalid gripper speed limits: min_speed_ or max_speed_ are not configured correctly.");
|
||||
}
|
||||
double scaled = (speed - min_speed_) / (max_speed_ - min_speed_) * 255.0;
|
||||
return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
|
||||
}
|
||||
|
||||
int RobotiqGripperHardwareInterface::convertForceToRaw(double force) {
|
||||
if (std::isnan(min_force_) || std::isnan(max_force_) || min_force_ >= max_force_) {
|
||||
throw std::runtime_error("Invalid gripper force limits: min_force_ or max_force_ are not configured correctly.");
|
||||
}
|
||||
double scaled = (force - min_force_) / (max_force_ - min_force_) * 255.0;
|
||||
return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
|
||||
}
|
||||
|
||||
} // namespace robotiq_interface
|
||||
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(robotiq_interface::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface)
|
||||
@ -1,69 +0,0 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include "RobotiqGripperHardwareInterface.hpp"
|
||||
|
||||
class TestableRobotiqGripperHardwareInterface : public robotiq_driver::RobotiqGripperHardwareInterface {
|
||||
public:
|
||||
using robotiq_driver::RobotiqGripperHardwareInterface::convertPositionToRaw;
|
||||
using robotiq_driver::RobotiqGripperHardwareInterface::convertRawToPosition;
|
||||
using robotiq_driver::RobotiqGripperHardwareInterface::convertSpeedToRaw;
|
||||
using robotiq_driver::RobotiqGripperHardwareInterface::convertForceToRaw;
|
||||
};
|
||||
|
||||
class RobotiqGripperConversionTests : public ::testing::Test {
|
||||
protected:
|
||||
TestableRobotiqGripperHardwareInterface gripper_interface;
|
||||
|
||||
void SetUp() override {
|
||||
gripper_interface.min_position_ = 0.0;
|
||||
gripper_interface.max_position_ = 0.14;
|
||||
gripper_interface.min_speed_ = 0.02;
|
||||
gripper_interface.max_speed_ = 0.15;
|
||||
gripper_interface.min_force_ = 20.0;
|
||||
gripper_interface.max_force_ = 235.0;
|
||||
}
|
||||
};
|
||||
|
||||
// Test for convertPositionToRaw
|
||||
TEST_F(RobotiqGripperConversionTests, PositionToRawValidInput) {
|
||||
EXPECT_EQ(gripper_interface.convertPositionToRaw(0.07), 128);
|
||||
}
|
||||
|
||||
TEST_F(RobotiqGripperConversionTests, PositionToRawBoundaryInput) {
|
||||
EXPECT_EQ(gripper_interface.convertPositionToRaw(0.0), 0);
|
||||
EXPECT_EQ(gripper_interface.convertPositionToRaw(0.14), 255);
|
||||
}
|
||||
|
||||
// Test for convertRawToPosition
|
||||
TEST_F(RobotiqGripperConversionTests, RawToPositionValidInput) {
|
||||
ASSERT_NEAR(gripper_interface.convertRawToPosition(128), 0.07, 0.01);
|
||||
}
|
||||
|
||||
TEST_F(RobotiqGripperConversionTests, RawToPositionBoundaryInput) {
|
||||
ASSERT_NEAR(gripper_interface.convertRawToPosition(0), 0.0, 0.01);
|
||||
ASSERT_NEAR(gripper_interface.convertRawToPosition(255), 0.14, 0.01);
|
||||
}
|
||||
|
||||
// Test for convertSpeedToRaw
|
||||
TEST_F(RobotiqGripperConversionTests, SpeedToRawValidInput) {
|
||||
EXPECT_EQ(gripper_interface.convertSpeedToRaw(0.085), 128);
|
||||
}
|
||||
|
||||
TEST_F(RobotiqGripperConversionTests, SpeedToRawBoundaryInput) {
|
||||
EXPECT_EQ(gripper_interface.convertSpeedToRaw(0.02), 0);
|
||||
EXPECT_EQ(gripper_interface.convertSpeedToRaw(0.15), 255);
|
||||
}
|
||||
|
||||
// Test for convertForceToRaw
|
||||
TEST_F(RobotiqGripperConversionTests, ForceToRawValidInput) {
|
||||
EXPECT_EQ(gripper_interface.convertForceToRaw(127.5), 128);
|
||||
}
|
||||
|
||||
TEST_F(RobotiqGripperConversionTests, ForceToRawBoundaryInput) {
|
||||
EXPECT_EQ(gripper_interface.convertForceToRaw(20.0), 0);
|
||||
EXPECT_EQ(gripper_interface.convertForceToRaw(235.0), 255);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
@ -1,9 +0,0 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
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
|
||||
@ -1,84 +0,0 @@
|
||||
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()
|
||||
@ -1,7 +0,0 @@
|
||||
<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>
|
||||
@ -1,64 +0,0 @@
|
||||
// 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
|
||||
@ -1,23 +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_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>
|
||||
@ -1,123 +0,0 @@
|
||||
// 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)
|
||||
1
src/ros2_robotiq_gripper
Submodule
1
src/ros2_robotiq_gripper
Submodule
@ -0,0 +1 @@
|
||||
Subproject commit b6136bdc866a929bfb096890ca61dde1335ffd30
|
||||
1
src/serial
Submodule
1
src/serial
Submodule
@ -0,0 +1 @@
|
||||
Subproject commit d8d160678aa0b31cdf467c052b954fa287cc6cdf
|
||||
67
src/servo_keyboard/CMakeLists.txt
Normal file
67
src/servo_keyboard/CMakeLists.txt
Normal file
@ -0,0 +1,67 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(servo_keyboard)
|
||||
|
||||
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(rclcpp REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(control_msgs REQUIRED)
|
||||
find_package(controller_manager_msgs REQUIRED)
|
||||
find_package(yaml-cpp REQUIRED)
|
||||
find_package(ament_index_cpp REQUIRED)
|
||||
|
||||
|
||||
add_executable(servo_keyboard_input src/servo_keyboard_input.cpp)
|
||||
target_include_directories(servo_keyboard_input PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
target_compile_features(servo_keyboard_input PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
|
||||
|
||||
# Include directories
|
||||
target_include_directories(servo_keyboard_input PRIVATE
|
||||
${rclcpp_INCLUDE_DIRS}
|
||||
${geometry_msgs_INCLUDE_DIRS}
|
||||
${control_msgs_INCLUDE_DIRS}
|
||||
${controller_manager_msgs_INCLUDE_DIRS}
|
||||
${YAML_CPP_INCLUDE_DIRS} # Add this line to include YAML-cpp headers
|
||||
)
|
||||
|
||||
# Link the executable with required libraries
|
||||
ament_target_dependencies(servo_keyboard_input
|
||||
rclcpp
|
||||
geometry_msgs
|
||||
control_msgs
|
||||
controller_manager_msgs
|
||||
yaml-cpp # Add this line to link against YAML-cpp
|
||||
ament_index_cpp
|
||||
)
|
||||
target_link_libraries(servo_keyboard_input ${YAML_CPP_LIBRARIES}) # Add this line to link against YAML-cpp
|
||||
|
||||
# Existing install directive for the executable
|
||||
install(TARGETS servo_keyboard_input
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
# Added install directive for the launch files
|
||||
install(DIRECTORY
|
||||
launch
|
||||
DESTINATION share/${PROJECT_NAME}/
|
||||
)
|
||||
|
||||
|
||||
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()
|
||||
202
src/servo_keyboard/LICENSE
Normal file
202
src/servo_keyboard/LICENSE
Normal file
@ -0,0 +1,202 @@
|
||||
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright [yyyy] [name of copyright owner]
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
6
src/servo_keyboard/config/servo_keyboard_params.yaml
Normal file
6
src/servo_keyboard/config/servo_keyboard_params.yaml
Normal file
@ -0,0 +1,6 @@
|
||||
/keyboard_servo:
|
||||
ros__parameters:
|
||||
joint_vel_cmd: 1.0
|
||||
gripper_speed_multiplier: 0.02
|
||||
gripper_lower_limit: 0.0
|
||||
gripper_upper_limit: 0.7
|
||||
16
src/servo_keyboard/launch/servo_keyboard.launch.py
Executable file
16
src/servo_keyboard/launch/servo_keyboard.launch.py
Executable file
@ -0,0 +1,16 @@
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import os
|
||||
def generate_launch_description():
|
||||
config_path = os.path.join(get_package_share_directory('servo_keyboard'), 'config', 'servo_keyboard_params.yaml')
|
||||
|
||||
return LaunchDescription([
|
||||
Node(
|
||||
package='servo_keyboard',
|
||||
executable='servo_keyboard_input',
|
||||
name='servo_keyboard_input',
|
||||
output='screen',
|
||||
parameters=[config_path],
|
||||
)
|
||||
])
|
||||
@ -1,18 +1,20 @@
|
||||
<?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_interface</name>
|
||||
<name>servo_keyboard</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="root@todo.todo">root</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
<maintainer email="ligerfotis@gmail.com">Fotios Lyegrakis</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<!-- Dependencies -->
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_lifecycle</depend>
|
||||
<depend>hardware_interface</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>control_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<build_depend>controller_manager_msgs</build_depend>
|
||||
<exec_depend>controller_manager_msgs</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
423
src/servo_keyboard/src/servo_keyboard_input.cpp
Normal file
423
src/servo_keyboard/src/servo_keyboard_input.cpp
Normal file
@ -0,0 +1,423 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2021, PickNik LLC
|
||||
* All rights reserved.
|
||||
*
|
||||
* 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 PickNik LLC 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 OWNER 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.
|
||||
*********************************************************************/
|
||||
|
||||
/* Title : servo_keyboard_input.cpp
|
||||
* Project : moveit2_tutorials
|
||||
* Created : 05/31/2021
|
||||
* Author : Adam Pettinger
|
||||
*/
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <geometry_msgs/msg/twist_stamped.hpp>
|
||||
#include <control_msgs/msg/joint_jog.hpp>
|
||||
#include <controller_manager_msgs/srv/switch_controller.hpp> // Add necessary includes
|
||||
#include <std_msgs/msg/float64_multi_array.hpp> // Add necessary includes
|
||||
#include <sensor_msgs/msg/joint_state.hpp> // Add necessary include for JointState message
|
||||
#include <ament_index_cpp/get_package_share_directory.hpp>
|
||||
|
||||
#include <signal.h>
|
||||
#include <stdio.h>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
|
||||
// Define used keys
|
||||
#define KEYCODE_LEFT 0x44
|
||||
#define KEYCODE_RIGHT 0x43
|
||||
#define KEYCODE_UP 0x41
|
||||
#define KEYCODE_DOWN 0x42
|
||||
#define KEYCODE_PERIOD 0x2E
|
||||
#define KEYCODE_SEMICOLON 0x3B
|
||||
#define KEYCODE_E 0x65
|
||||
#define KEYCODE_W 0x77
|
||||
#define KEYCODE_1 0x31
|
||||
#define KEYCODE_2 0x32
|
||||
#define KEYCODE_3 0x33
|
||||
#define KEYCODE_4 0x34
|
||||
#define KEYCODE_5 0x35
|
||||
#define KEYCODE_6 0x36
|
||||
#define KEYCODE_R 0x72
|
||||
#define KEYCODE_Q 0x71
|
||||
#define KEYCODE_PLUS 0x2B // Keycode for the plus sign (+)
|
||||
#define KEYCODE_MINUS 0x2D // Keycode for the minus sign (-)
|
||||
#define KEYCODE_GRIPPER 0x67 // Keycode for the gripper control button (g)
|
||||
|
||||
// Some constants used in the Servo Teleop demo
|
||||
const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds";
|
||||
const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds";
|
||||
const std::string GRIPPER_TOPIC = "/forward_gripper_position_controller/commands";
|
||||
const size_t ROS_QUEUE_SIZE = 10;
|
||||
const std::string EEF_FRAME_ID = "world";
|
||||
const std::string BASE_FRAME_ID = "tool0";
|
||||
|
||||
// A class for reading the key inputs from the terminal
|
||||
class KeyboardReader
|
||||
{
|
||||
public:
|
||||
KeyboardReader() : kfd(0)
|
||||
{
|
||||
// get the console in raw mode
|
||||
tcgetattr(kfd, &cooked);
|
||||
struct termios raw;
|
||||
memcpy(&raw, &cooked, sizeof(struct termios));
|
||||
raw.c_lflag &= ~(ICANON | ECHO);
|
||||
// Setting a new line, then end of file
|
||||
raw.c_cc[VEOL] = 1;
|
||||
raw.c_cc[VEOF] = 2;
|
||||
tcsetattr(kfd, TCSANOW, &raw);
|
||||
}
|
||||
void readOne(char* c)
|
||||
{
|
||||
int rc = read(kfd, c, 1);
|
||||
if (rc < 0)
|
||||
{
|
||||
throw std::runtime_error("read failed");
|
||||
}
|
||||
}
|
||||
void shutdown()
|
||||
{
|
||||
tcsetattr(kfd, TCSANOW, &cooked);
|
||||
}
|
||||
|
||||
private:
|
||||
int kfd;
|
||||
struct termios cooked;
|
||||
};
|
||||
|
||||
// Converts key-presses to Twist or Jog commands for Servo, in lieu of a controller
|
||||
class KeyboardServo
|
||||
{
|
||||
public:
|
||||
KeyboardServo();
|
||||
int keyLoop();
|
||||
|
||||
private:
|
||||
void spin();
|
||||
|
||||
void handlePlusPress();
|
||||
void handleMinusPress();
|
||||
void jointStateCallback(sensor_msgs::msg::JointState::SharedPtr msg); // Declaration of jointStateCallback
|
||||
void publishGripperCommand(double finger_joint_angle);
|
||||
|
||||
rclcpp::Node::SharedPtr nh_;
|
||||
|
||||
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
|
||||
rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_pub_;
|
||||
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr gripper_cmd_pub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
|
||||
|
||||
std::string frame_to_publish_;
|
||||
double joint_vel_cmd_;
|
||||
double gripper_speed_multiplier_;
|
||||
double last_finger_joint_angle_;
|
||||
double gripper_lower_limit_;
|
||||
double gripper_upper_limit_;
|
||||
};
|
||||
|
||||
KeyboardServo::KeyboardServo() {
|
||||
|
||||
// Create the node
|
||||
nh_ = rclcpp::Node::make_shared("keyboard_servo");
|
||||
|
||||
// Declare the parameters
|
||||
nh_->declare_parameter("joint_vel_cmd", 1.0);
|
||||
nh_->declare_parameter("gripper_speed_multiplier", 0.1);
|
||||
nh_->declare_parameter("gripper_lower_limit", 0.1);
|
||||
nh_->declare_parameter("gripper_upper_limit", 0.4);
|
||||
|
||||
// Get the parameters from the node
|
||||
nh_->get_parameter("joint_vel_cmd", joint_vel_cmd_);
|
||||
nh_->get_parameter("gripper_speed_multiplier", gripper_speed_multiplier_);
|
||||
nh_->get_parameter("gripper_lower_limit", gripper_lower_limit_);
|
||||
nh_->get_parameter("gripper_upper_limit", gripper_upper_limit_);
|
||||
|
||||
// Print the parameters
|
||||
printf("Joint velocity command: %f\n", joint_vel_cmd_);
|
||||
printf("Gripper speed multiplier: %f\n", gripper_speed_multiplier_);
|
||||
printf("Gripper lower limit: %f\n", gripper_lower_limit_);
|
||||
printf("Gripper upper limit: %f\n", gripper_upper_limit_);
|
||||
|
||||
twist_pub_ = nh_->create_publisher<geometry_msgs::msg::TwistStamped>(TWIST_TOPIC, ROS_QUEUE_SIZE);
|
||||
joint_pub_ = nh_->create_publisher<control_msgs::msg::JointJog>(JOINT_TOPIC, ROS_QUEUE_SIZE);
|
||||
gripper_cmd_pub_ = nh_->create_publisher<std_msgs::msg::Float64MultiArray>(GRIPPER_TOPIC, ROS_QUEUE_SIZE);
|
||||
joint_state_sub_ = nh_->create_subscription<sensor_msgs::msg::JointState>(
|
||||
"/joint_states", ROS_QUEUE_SIZE,
|
||||
std::bind(&KeyboardServo::jointStateCallback, this, std::placeholders::_1));
|
||||
|
||||
}
|
||||
|
||||
|
||||
KeyboardReader input;
|
||||
|
||||
// Implement the jointStateCallback function
|
||||
void KeyboardServo::jointStateCallback(sensor_msgs::msg::JointState::SharedPtr msg)
|
||||
{
|
||||
// Find the index of the finger joint in the message
|
||||
auto it = std::find(msg->name.begin(), msg->name.end(), "finger_joint");
|
||||
if (it != msg->name.end())
|
||||
{
|
||||
size_t index = std::distance(msg->name.begin(), it);
|
||||
// Set the last finger joint angle based on the received message
|
||||
last_finger_joint_angle_ = msg->position[index];
|
||||
}
|
||||
}
|
||||
|
||||
void KeyboardServo::publishGripperCommand(double finger_joint_angle)
|
||||
{
|
||||
auto msg = std::make_unique<std_msgs::msg::Float64MultiArray>();
|
||||
msg->data.push_back(finger_joint_angle);
|
||||
gripper_cmd_pub_->publish(std::move(msg));
|
||||
}
|
||||
|
||||
void quit(int sig)
|
||||
{
|
||||
(void)sig;
|
||||
input.shutdown();
|
||||
rclcpp::shutdown();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
KeyboardServo keyboard_servo;
|
||||
|
||||
signal(SIGINT, quit);
|
||||
|
||||
int rc = keyboard_servo.keyLoop();
|
||||
input.shutdown();
|
||||
rclcpp::shutdown();
|
||||
|
||||
return rc;
|
||||
}
|
||||
|
||||
void KeyboardServo::spin()
|
||||
{
|
||||
while (rclcpp::ok())
|
||||
{
|
||||
rclcpp::spin_some(nh_);
|
||||
}
|
||||
}
|
||||
|
||||
void KeyboardServo::handlePlusPress()
|
||||
{
|
||||
// Calculate the new finger joint angle
|
||||
double delta_angle = 1.0 * gripper_speed_multiplier_;
|
||||
double new_finger_joint_angle = last_finger_joint_angle_ + delta_angle;
|
||||
// printf("New finger joint angle: %f\n", new_finger_joint_angle);
|
||||
// Check if the new angle is within the limits
|
||||
if (new_finger_joint_angle <= gripper_upper_limit_)
|
||||
{
|
||||
// Update the finger joint angle
|
||||
last_finger_joint_angle_ = new_finger_joint_angle;
|
||||
// printf("New finger joint angle: %f\n", last_finger_joint_angle_);
|
||||
// Publish the gripper command with the new angle
|
||||
publishGripperCommand(last_finger_joint_angle_);
|
||||
}
|
||||
}
|
||||
|
||||
void KeyboardServo::handleMinusPress()
|
||||
{
|
||||
// Calculate the new finger joint angle
|
||||
double delta_angle = -1.0 * gripper_speed_multiplier_;
|
||||
double new_finger_joint_angle = last_finger_joint_angle_ + delta_angle;
|
||||
|
||||
// Check if the new angle is within the limits
|
||||
if (new_finger_joint_angle >= gripper_lower_limit_)
|
||||
{
|
||||
// Update the finger joint angle
|
||||
last_finger_joint_angle_ = new_finger_joint_angle;
|
||||
|
||||
// Publish the gripper command with the new angle
|
||||
publishGripperCommand(last_finger_joint_angle_);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
int KeyboardServo::keyLoop()
|
||||
{
|
||||
char c;
|
||||
bool publish_twist = false;
|
||||
bool publish_joint = false;
|
||||
|
||||
std::thread{ std::bind(&KeyboardServo::spin, this) }.detach();
|
||||
|
||||
puts("Reading from keyboard");
|
||||
puts("---------------------------");
|
||||
puts("Use arrow keys and the '.' and ';' keys to Cartesian jog");
|
||||
puts("Use 'W' to Cartesian jog in the world frame, and 'E' for the End-Effector frame");
|
||||
puts("Use 1|2|3|4|5|6|7 keys to joint jog. 'R' to reverse the direction of jogging.");
|
||||
puts("Use '+' to open the gripper, '-' to close it.");
|
||||
puts("'Q' to quit.");
|
||||
|
||||
|
||||
for (;;)
|
||||
{
|
||||
// get the next event from the keyboard
|
||||
try
|
||||
{
|
||||
input.readOne(&c);
|
||||
}
|
||||
catch (const std::runtime_error&)
|
||||
{
|
||||
perror("read():");
|
||||
return -1;
|
||||
}
|
||||
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "value: 0x%02X\n", c);
|
||||
|
||||
// // Create the messages we might publish
|
||||
auto twist_msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
|
||||
auto joint_msg = std::make_unique<control_msgs::msg::JointJog>();
|
||||
|
||||
// Use read key-press
|
||||
switch (c)
|
||||
{
|
||||
case KEYCODE_LEFT:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "LEFT");
|
||||
twist_msg->twist.linear.y = -1.0;
|
||||
publish_twist = true;
|
||||
break;
|
||||
case KEYCODE_RIGHT:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "RIGHT");
|
||||
twist_msg->twist.linear.y = 1.0;
|
||||
publish_twist = true;
|
||||
break;
|
||||
case KEYCODE_UP:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "UP");
|
||||
twist_msg->twist.linear.x = 1.0;
|
||||
publish_twist = true;
|
||||
break;
|
||||
case KEYCODE_DOWN:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "DOWN");
|
||||
twist_msg->twist.linear.x = -1.0;
|
||||
publish_twist = true;
|
||||
break;
|
||||
case KEYCODE_PERIOD:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "PERIOD");
|
||||
twist_msg->twist.linear.z = -1.0;
|
||||
publish_twist = true;
|
||||
break;
|
||||
case KEYCODE_SEMICOLON:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "SEMICOLON");
|
||||
twist_msg->twist.linear.z = 1.0;
|
||||
publish_twist = true;
|
||||
break;
|
||||
case KEYCODE_E:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "E");
|
||||
frame_to_publish_ = EEF_FRAME_ID;
|
||||
break;
|
||||
case KEYCODE_W:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "W");
|
||||
frame_to_publish_ = BASE_FRAME_ID;
|
||||
break;
|
||||
case KEYCODE_1:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "1");
|
||||
joint_msg->joint_names.push_back("shoulder_lift_joint");
|
||||
joint_msg->velocities.push_back(joint_vel_cmd_);
|
||||
publish_joint = true;
|
||||
break;
|
||||
case KEYCODE_2:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "2");
|
||||
joint_msg->joint_names.push_back("shoulder_pan_joint");
|
||||
joint_msg->velocities.push_back(joint_vel_cmd_);
|
||||
publish_joint = true;
|
||||
break;
|
||||
case KEYCODE_3:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "3");
|
||||
joint_msg->joint_names.push_back("elbow_joint");
|
||||
joint_msg->velocities.push_back(joint_vel_cmd_);
|
||||
publish_joint = true;
|
||||
break;
|
||||
case KEYCODE_4:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "4");
|
||||
joint_msg->joint_names.push_back("wrist_1_joint");
|
||||
joint_msg->velocities.push_back(joint_vel_cmd_);
|
||||
publish_joint = true;
|
||||
break;
|
||||
case KEYCODE_5:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "5");
|
||||
joint_msg->joint_names.push_back("wrist_2_joint");
|
||||
joint_msg->velocities.push_back(joint_vel_cmd_);
|
||||
publish_joint = true;
|
||||
break;
|
||||
case KEYCODE_6:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "6");
|
||||
joint_msg->joint_names.push_back("wrist_3_joint");
|
||||
joint_msg->velocities.push_back(joint_vel_cmd_);
|
||||
publish_joint = true;
|
||||
break;
|
||||
// case KEYCODE_7:
|
||||
// RCLCPP_DEBUG(nh_->get_logger(), "7");
|
||||
// joint_msg->joint_names.push_back("finger_joint");
|
||||
// joint_msg->velocities.push_back(joint_vel_cmd_);
|
||||
// publish_joint = true;
|
||||
// break;
|
||||
case KEYCODE_R:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "R");
|
||||
joint_vel_cmd_ *= -1;
|
||||
break;
|
||||
case KEYCODE_Q:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "quit");
|
||||
return 0;
|
||||
// Add cases for other keys as needed
|
||||
case KEYCODE_PLUS:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "PLUS");
|
||||
handlePlusPress();
|
||||
break;
|
||||
case KEYCODE_MINUS:
|
||||
RCLCPP_DEBUG(nh_->get_logger(), "MINUS");
|
||||
handleMinusPress();
|
||||
break;
|
||||
}
|
||||
|
||||
// If a key requiring a publish was pressed, publish the message now
|
||||
if (publish_twist)
|
||||
{
|
||||
twist_msg->header.stamp = nh_->now();
|
||||
twist_msg->header.frame_id = frame_to_publish_;
|
||||
twist_pub_->publish(std::move(twist_msg));
|
||||
publish_twist = false;
|
||||
}
|
||||
else if (publish_joint)
|
||||
{
|
||||
joint_msg->header.stamp = nh_->now();
|
||||
joint_msg->header.frame_id = BASE_FRAME_ID;
|
||||
joint_pub_->publish(std::move(joint_msg));
|
||||
publish_joint = false;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -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
|
||||
@ -6,7 +6,8 @@ from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, TextSubstitution
|
||||
|
||||
# from moveit_configs_utils import MoveItConfigsBuilder
|
||||
# from launch_param_builder import ParameterBuilder
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
# Initialize Arguments
|
||||
@ -170,6 +171,7 @@ def launch_setup(context, *args, **kwargs):
|
||||
|
||||
robot_description = {"robot_description": robot_description_content}
|
||||
|
||||
|
||||
initial_joint_controllers = PathJoinSubstitution(
|
||||
[FindPackageShare(description_package), "config", controllers_file]
|
||||
)
|
||||
@ -275,6 +277,38 @@ def launch_setup(context, *args, **kwargs):
|
||||
},
|
||||
],
|
||||
)
|
||||
############################################################################################################
|
||||
# # Get parameters for the Servo node
|
||||
# servo_params = (
|
||||
# ParameterBuilder("ur_robotiq_servo")
|
||||
# .yaml(
|
||||
# parameter_namespace="moveit_servo",
|
||||
# file_path="config/ur_robotiq_simulated_config.yaml",
|
||||
# )
|
||||
# .to_dict()
|
||||
# )
|
||||
# print(servo_params)
|
||||
#
|
||||
# # A node to publish world -> panda_link0 transform
|
||||
# static_tf = Node(
|
||||
# package="tf2_ros",
|
||||
# executable="static_transform_publisher",
|
||||
# name="static_transform_publisher",
|
||||
# output="log",
|
||||
# arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
|
||||
# )
|
||||
#
|
||||
# # The servo cpp interface demo
|
||||
# # Creates the Servo node and publishes commands to it
|
||||
# servo_node = Node(
|
||||
# package="moveit2_tutorials",
|
||||
# executable="servo_cpp_interface_demo",
|
||||
# output="screen",
|
||||
# parameters=[
|
||||
# servo_params,
|
||||
# robot_description,
|
||||
# ],
|
||||
# )
|
||||
|
||||
robot_state_publisher_node = Node(
|
||||
package="robot_state_publisher",
|
||||
@ -353,6 +387,8 @@ def launch_setup(context, *args, **kwargs):
|
||||
ur_control_node,
|
||||
dashboard_client_node,
|
||||
controller_stopper_node,
|
||||
# static_tf,
|
||||
# servo_node,
|
||||
robot_state_publisher_node,
|
||||
rviz_node,
|
||||
initial_joint_controller_spawner_stopped,
|
||||
|
||||
@ -1,9 +1,9 @@
|
||||
<?xml version="1.0" ?>
|
||||
<!-- =================================================================================== -->
|
||||
<!-- | This document was autogenerated by xacro from ur_robotiq.urdf.xacro | -->
|
||||
<!-- | This document was autogenerated by xacro from src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro | -->
|
||||
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||
<!-- =================================================================================== -->
|
||||
<robot name="ur_manipulator">
|
||||
<robot name="ur">
|
||||
<!--
|
||||
Base UR robot series xacro macro.
|
||||
|
||||
@ -59,7 +59,7 @@
|
||||
this location. Nor can they rely on the existence of the macro.
|
||||
-->
|
||||
<link name="world"/>
|
||||
<ros2_control name="ur_manipulator" type="system">
|
||||
<ros2_control name="ur" type="system">
|
||||
<hardware>
|
||||
<plugin>ur_robot_driver/URPositionHardwareInterface</plugin>
|
||||
<param name="robot_ip">0.0.0.0</param>
|
||||
@ -99,7 +99,7 @@
|
||||
</command_interface>
|
||||
<state_interface name="position">
|
||||
<!-- initial position for the FakeSystem and simulation -->
|
||||
<param name="initial_value">0.0</param>
|
||||
<param name="initial_value">1.4</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
@ -131,7 +131,7 @@
|
||||
</command_interface>
|
||||
<state_interface name="position">
|
||||
<!-- initial position for the FakeSystem and simulation -->
|
||||
<param name="initial_value">0.0</param>
|
||||
<param name="initial_value">1.57</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
@ -163,7 +163,7 @@
|
||||
</command_interface>
|
||||
<state_interface name="position">
|
||||
<!-- initial position for the FakeSystem and simulation -->
|
||||
<param name="initial_value">0.0</param>
|
||||
<param name="initial_value">-1.57</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="effort"/>
|
||||
@ -591,7 +591,7 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_base_link.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_base_link.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
@ -600,7 +600,7 @@
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_base_link.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_base_link.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@ -618,7 +618,7 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
@ -627,7 +627,7 @@
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@ -640,7 +640,7 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
@ -649,7 +649,7 @@
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@ -662,7 +662,7 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
@ -671,7 +671,7 @@
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@ -704,7 +704,7 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
@ -713,7 +713,7 @@
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@ -726,7 +726,7 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
|
||||
@ -735,7 +735,7 @@
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@ -748,7 +748,7 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
@ -757,7 +757,7 @@
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@ -770,7 +770,7 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
@ -779,7 +779,7 @@
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
@ -812,7 +812,7 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.1 0.1 0.1 1"/>
|
||||
@ -821,7 +821,7 @@
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||
<mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
@ -5,7 +5,6 @@
|
||||
<xacro:arg name="name" default="ur_manipulator"/>
|
||||
<!-- import main macro -->
|
||||
<xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>
|
||||
<xacro:include filename="$(find ur_robotiq_description)/urdf/ur_to_robotiq_adapter.urdf.xacro" />
|
||||
<xacro:include filename="$(find robotiq_description)/urdf/robotiq_2f_140_macro.urdf.xacro" />
|
||||
|
||||
<!-- possible 'ur_type' values: ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e, ur20 -->
|
||||
|
||||
@ -1,39 +0,0 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="ur_robotiq_adapter">
|
||||
<xacro:macro name="ur_to_robotiq" params="prefix parent child rotation:=^|${0.0}">
|
||||
|
||||
<joint name="${prefix}ur_to_robotiq_joint" type="fixed">
|
||||
<!-- The parent link must be read from the robot model it is attached to. -->
|
||||
<parent link="${parent}"/>
|
||||
<child link="${prefix}ur_to_robotiq_link"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 ${rotation}"/>
|
||||
</joint>
|
||||
|
||||
<link name="${prefix}ur_to_robotiq_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_description/meshes/visual/2f_85/ur_to_robotiq_adapter.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://robotiq_description/meshes/collision/2f_85/ur_to_robotiq_adapter.stl"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.01" />
|
||||
<origin xyz="0 0 0" />
|
||||
<inertia ixx="0.000044" ixy="0.0" ixz="0.0" iyy="0.000027" iyz="0.0" izz="0.000027" />
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="${prefix}gripper_side_joint" type="fixed">
|
||||
<parent link="${prefix}ur_to_robotiq_link"/>
|
||||
<child link="${child}"/>
|
||||
<!-- <origin xyz="0 0 0.011" rpy="0 ${-pi/2} ${pi/2}"/> -->
|
||||
<origin xyz="0 0 0.011" rpy="0 0 0"/>
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
0
src/ur_robotiq_servo/__init__.py
Normal file
0
src/ur_robotiq_servo/__init__.py
Normal file
@ -1,7 +1,8 @@
|
||||
joy_node:
|
||||
ros__parameters:
|
||||
device_id: 0
|
||||
device_name: "PS5 Controller"
|
||||
# device_name: "PS5 Controller"
|
||||
device_name: "Keyboard"
|
||||
deadzone: 0.5
|
||||
autorepeat_rate: 20.0
|
||||
sticky_buttons: false
|
||||
|
||||
6
src/ur_robotiq_servo/config/kb-params.yaml
Normal file
6
src/ur_robotiq_servo/config/kb-params.yaml
Normal file
@ -0,0 +1,6 @@
|
||||
keyboard_servo_node:
|
||||
ros__parameters:
|
||||
linear_speed_multiplier: 0.2
|
||||
gripper_speed_multiplier: 0.02
|
||||
gripper_lower_limit: 0.0
|
||||
gripper_upper_limit: 0.70
|
||||
46
src/ur_robotiq_servo/launch/kb_servo.launch.py
Normal file
46
src/ur_robotiq_servo/launch/kb_servo.launch.py
Normal file
@ -0,0 +1,46 @@
|
||||
import os
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
import ament_index_python.packages
|
||||
from launch_param_builder import ParameterBuilder
|
||||
from moveit_configs_utils import MoveItConfigsBuilder
|
||||
|
||||
def generate_launch_description():
|
||||
config_directory = os.path.join(
|
||||
ament_index_python.packages.get_package_share_directory('ur_robotiq_servo'),
|
||||
'config')
|
||||
joy_params = os.path.join(config_directory, 'joy-params.yaml')
|
||||
ps5_params = os.path.join(config_directory, 'ps5-params.yaml')
|
||||
|
||||
moveit_config = (
|
||||
MoveItConfigsBuilder("moveit_resources_panda")
|
||||
.robot_description(file_path="config/panda.urdf.xacro")
|
||||
.to_moveit_configs()
|
||||
)
|
||||
# Get parameters for the Servo node
|
||||
servo_params = (
|
||||
ParameterBuilder("moveit_servo")
|
||||
.yaml(
|
||||
parameter_namespace="moveit_servo",
|
||||
file_path="config/panda_simulated_config.yaml",
|
||||
)
|
||||
.to_dict()
|
||||
)
|
||||
|
||||
# The servo cpp interface demo
|
||||
# Creates the Servo node and publishes commands to it
|
||||
servo_node = Node(
|
||||
package="moveit2_tutorials",
|
||||
executable="servo_cpp_interface_demo",
|
||||
output="screen",
|
||||
parameters=[
|
||||
servo_params,
|
||||
moveit_config.robot_description,
|
||||
moveit_config.robot_description_semantic,
|
||||
],
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
servo_node
|
||||
])
|
||||
@ -14,6 +14,7 @@
|
||||
<depend>std_srvs</depend>
|
||||
<depend>joy</depend>
|
||||
<depend>moveit_servo</depend>
|
||||
<depend>keyboard</depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
|
||||
196
src/ur_robotiq_servo/servo_keyboard_input.py
Normal file
196
src/ur_robotiq_servo/servo_keyboard_input.py
Normal file
@ -0,0 +1,196 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import sys
|
||||
import tty
|
||||
import termios
|
||||
import threading
|
||||
import signal
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from geometry_msgs.msg import TwistStamped
|
||||
from control_msgs.msg import JointJog
|
||||
|
||||
# Define key codes
|
||||
KEYCODE_RIGHT = 0x43
|
||||
KEYCODE_LEFT = 0x44
|
||||
KEYCODE_UP = 0x41
|
||||
KEYCODE_DOWN = 0x42
|
||||
KEYCODE_PERIOD = 0x2E
|
||||
KEYCODE_SEMICOLON = 0x3B
|
||||
KEYCODE_1 = 0x31
|
||||
KEYCODE_2 = 0x32
|
||||
KEYCODE_3 = 0x33
|
||||
KEYCODE_4 = 0x34
|
||||
KEYCODE_5 = 0x35
|
||||
KEYCODE_6 = 0x36
|
||||
KEYCODE_7 = 0x37
|
||||
KEYCODE_Q = 0x71
|
||||
KEYCODE_R = 0x72
|
||||
KEYCODE_J = 0x6A
|
||||
KEYCODE_T = 0x74
|
||||
KEYCODE_W = 0x77
|
||||
KEYCODE_E = 0x65
|
||||
|
||||
# Constants used in the Servo Teleop demo
|
||||
TWIST_TOPIC = "/servo_node/delta_twist_cmds"
|
||||
JOINT_TOPIC = "/servo_node/delta_joint_cmds"
|
||||
ROS_QUEUE_SIZE = 10
|
||||
PLANNING_FRAME_ID = "world"
|
||||
EE_FRAME_ID = "tool0"
|
||||
|
||||
|
||||
class KeyboardReader:
|
||||
def __init__(self):
|
||||
self.file_descriptor = sys.stdin.fileno()
|
||||
self.old_settings = termios.tcgetattr(self.file_descriptor)
|
||||
tty.setraw(self.file_descriptor)
|
||||
|
||||
def read_one(self):
|
||||
return sys.stdin.read(1)
|
||||
|
||||
def shutdown(self):
|
||||
termios.tcsetattr(self.file_descriptor, termios.TCSADRAIN, self.old_settings)
|
||||
|
||||
|
||||
class KeyboardServo:
|
||||
def __init__(self):
|
||||
self.joint_vel_cmd = 1.0
|
||||
self.command_frame_id = PLANNING_FRAME_ID
|
||||
self.node = rclpy.create_node("servo_keyboard_input")
|
||||
self.twist_pub = self.node.create_publisher(TwistStamped, TWIST_TOPIC, ROS_QUEUE_SIZE)
|
||||
self.joint_pub = self.node.create_publisher(JointJog, JOINT_TOPIC, ROS_QUEUE_SIZE)
|
||||
self.switch_input = self.node.create_client(ServoCommandType, "servo_node/switch_command_type")
|
||||
self.request = ServoCommandType.Request()
|
||||
|
||||
def spin(self):
|
||||
rclpy.spin(self.node)
|
||||
|
||||
def key_loop(self):
|
||||
publish_twist = False
|
||||
publish_joint = False
|
||||
print("Reading from keyboard")
|
||||
print("---------------------------")
|
||||
print("All commands are in the planning frame")
|
||||
print("Use arrow keys and the '.' and ';' keys to Cartesian jog")
|
||||
print("Use 1|2|3|4|5|6|7 keys to joint jog. 'r' to reverse the direction of jogging.")
|
||||
print("Use 'j' to select joint jog.")
|
||||
print("Use 't' to select twist")
|
||||
print("Use 'w' and 'e' to switch between sending command in planning frame or end effector frame")
|
||||
print("'Q' to quit.")
|
||||
|
||||
while True:
|
||||
c = input()
|
||||
|
||||
twist_msg = TwistStamped()
|
||||
joint_msg = JointJog()
|
||||
joint_msg.joint_names = [
|
||||
"shoulder_lift_joint",
|
||||
"shoulder_pan_joint",
|
||||
"elbow_joint",
|
||||
"wrist_1_joint",
|
||||
"wrist_2_joint",
|
||||
"wrist_2_joint",
|
||||
"finger_joint",
|
||||
]
|
||||
joint_msg.velocities = [0.0] * 7
|
||||
|
||||
# Use read key-press
|
||||
if c == '\x1b': # ANSI escape sequence
|
||||
c = input()
|
||||
if c == '[':
|
||||
c = input()
|
||||
if c == 'A':
|
||||
twist_msg.twist.linear.x = 0.5 # UP
|
||||
publish_twist = True
|
||||
elif c == 'B':
|
||||
twist_msg.twist.linear.x = -0.5 # DOWN
|
||||
publish_twist = True
|
||||
elif c == 'C':
|
||||
twist_msg.twist.linear.y = 0.5 # RIGHT
|
||||
publish_twist = True
|
||||
elif c == 'D':
|
||||
twist_msg.twist.linear.y = -0.5 # LEFT
|
||||
publish_twist = True
|
||||
elif ord(c) == KEYCODE_PERIOD:
|
||||
twist_msg.twist.linear.z = -0.5 # '.'
|
||||
publish_twist = True
|
||||
elif ord(c) == KEYCODE_SEMICOLON:
|
||||
twist_msg.twist.linear.z = 0.5 # ';'
|
||||
publish_twist = True
|
||||
elif ord(c) == KEYCODE_1:
|
||||
joint_msg.velocities[0] = self.joint_vel_cmd # '1'
|
||||
publish_joint = True
|
||||
elif ord(c) == KEYCODE_2:
|
||||
joint_msg.velocities[1] = self.joint_vel_cmd # '2'
|
||||
publish_joint = True
|
||||
elif ord(c) == KEYCODE_3:
|
||||
joint_msg.velocities[2] = self.joint_vel_cmd # '3'
|
||||
publish_joint = True
|
||||
elif ord(c) == KEYCODE_4:
|
||||
joint_msg.velocities[3] = self.joint_vel_cmd # '4'
|
||||
publish_joint = True
|
||||
elif ord(c) == KEYCODE_5:
|
||||
joint_msg.velocities[4] = self.joint_vel_cmd # '5'
|
||||
publish_joint = True
|
||||
elif ord(c) == KEYCODE_6:
|
||||
joint_msg.velocities[5] = self.joint_vel_cmd # '6'
|
||||
publish_joint = True
|
||||
elif ord(c) == KEYCODE_7:
|
||||
joint_msg.velocities[6] = self.joint_vel_cmd # '7'
|
||||
publish_joint = True
|
||||
elif ord(c) == KEYCODE_R:
|
||||
self.joint_vel_cmd *= -1 # 'r'
|
||||
elif ord(c) == KEYCODE_J:
|
||||
self.request.command_type = ServoCommandType.Request.JOINT_JOG # 'j'
|
||||
if self.switch_input.wait_for_service(timeout_sec=1):
|
||||
result = self.switch_input.call(self.request)
|
||||
if result.success:
|
||||
print("Switched to input type: JointJog")
|
||||
else:
|
||||
print("Could not switch input to: JointJog")
|
||||
elif ord(c) == KEYCODE_T:
|
||||
self.request.command_type = ServoCommandType.Request.TWIST # 't'
|
||||
if self.switch_input.wait_for_service(timeout_sec=1):
|
||||
result = self.switch_input.call(self.request)
|
||||
if result.success:
|
||||
print("Switched to input type: Twist")
|
||||
else:
|
||||
print("Could not switch input to: Twist")
|
||||
elif ord(c) == KEYCODE_W:
|
||||
print(f"Command frame set to: {PLANNING_FRAME_ID}") # 'w'
|
||||
self.command_frame_id = PLANNING_FRAME_ID
|
||||
elif ord(c) == KEYCODE_E:
|
||||
print(f"Command frame set to: {EE_FRAME_ID}") # 'e'
|
||||
self.command_frame_id = EE_FRAME_ID
|
||||
elif ord(c) == KEYCODE_Q:
|
||||
print("Quit") # 'Q'
|
||||
return 0
|
||||
|
||||
# If a key requiring a publish was pressed, publish the message now
|
||||
if publish_twist:
|
||||
twist_msg.header.stamp = self.node.get_clock().now().to_msg()
|
||||
twist_msg.header.frame_id = self.command_frame_id
|
||||
self.twist_pub.publish(twist_msg)
|
||||
publish_twist = False
|
||||
elif publish_joint:
|
||||
joint_msg.header.stamp = self.node.get_clock().now().to_msg()
|
||||
joint_msg.header.frame_id = PLANNING_FRAME_ID
|
||||
self.joint_pub.publish(joint_msg)
|
||||
publish_joint = False
|
||||
|
||||
return 0
|
||||
|
||||
|
||||
def quit_handler(sig, frame):
|
||||
input_reader.shutdown()
|
||||
rclpy.shutdown()
|
||||
sys.exit(0)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
signal.signal(signal.SIGINT, quit_handler)
|
||||
rclpy.init(args=sys.argv)
|
||||
input_reader = KeyboardReader()
|
||||
servo_keyboard = KeyboardServo()
|
||||
threading.Thread(target=servo_keyboard.spin).start()
|
||||
servo_keyboard.key_loop()
|
||||
@ -24,7 +24,9 @@ setup(
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'ps5_servo = ur_robotiq_servo.ps5_control:main'
|
||||
'ps5_servo = ur_robotiq_servo.ps5_control:main',
|
||||
'kb_servo = ur_robotiq_servo.kb_control:main',
|
||||
# 'servo_kb_input = ur_robotiq_servo.servo_keyboard_input:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
142
src/ur_robotiq_servo/ur_robotiq_servo/kb_control.py
Normal file
142
src/ur_robotiq_servo/ur_robotiq_servo/kb_control.py
Normal file
@ -0,0 +1,142 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import Joy, JointState
|
||||
from geometry_msgs.msg import TwistStamped
|
||||
from control_msgs.msg import JointJog
|
||||
from std_srvs.srv import Trigger
|
||||
from std_msgs.msg import Float64MultiArray, MultiArrayDimension, MultiArrayLayout
|
||||
import keyboard
|
||||
|
||||
class KBControllerNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__('kb_controller_node')
|
||||
# States
|
||||
self.mode = 'twist' # Initialize mode to 'twist'. Alternatives: 'twist', 'joint'
|
||||
self.last_button_state = 0 # Track the last state of the toggle button to detect presses
|
||||
self.last_finger_joint_angle = 0.0
|
||||
|
||||
# Parameters
|
||||
self.linear_speed_multiplier = self.declare_parameter('linear_speed_multiplier', 1.0)
|
||||
self.linear_speed_multiplier = self.get_parameter('linear_speed_multiplier').get_parameter_value().double_value
|
||||
self.get_logger().info(f"Linear speed multiplier: {self.linear_speed_multiplier}")
|
||||
|
||||
self.use_fake_hardware = self.declare_parameter('use_fake_hardware', False)
|
||||
self.use_fake_hardware = self.get_parameter('use_fake_hardware').get_parameter_value().bool_value
|
||||
self.get_logger().info(f"Use fake hardware: {self.use_fake_hardware}")
|
||||
|
||||
self.gripper_speed_multiplier = self.declare_parameter('gripper_speed_multiplier', 1.0)
|
||||
self.gripper_speed_multiplier = (self.get_parameter('gripper_speed_multiplier')
|
||||
.get_parameter_value().double_value)
|
||||
self.get_logger().info(f"Gripper speed multiplier: {self.gripper_speed_multiplier}")
|
||||
|
||||
self.gripper_lower_limit = self.declare_parameter('gripper_lower_limit', 1.0)
|
||||
self.gripper_lower_limit = (self.get_parameter('gripper_lower_limit')
|
||||
.get_parameter_value().double_value)
|
||||
self.get_logger().info(f"Gripper lower limit: {self.gripper_lower_limit}")
|
||||
|
||||
self.gripper_upper_limit = self.declare_parameter('gripper_upper_limit', 1.0)
|
||||
self.gripper_upper_limit = (self.get_parameter('gripper_upper_limit')
|
||||
.get_parameter_value().double_value)
|
||||
self.get_logger().info(f"Gripper upper limit: {self.gripper_upper_limit}")
|
||||
# Subscriber and Publisher
|
||||
self.joint_state_sub = self.create_subscription(
|
||||
JointState,
|
||||
'/joint_states',
|
||||
self.joint_state_callback,
|
||||
10)
|
||||
|
||||
self.joy_sub = self.create_subscription(
|
||||
Joy,
|
||||
'/joy',
|
||||
self.keyboard_callback,
|
||||
10)
|
||||
|
||||
self.twist_pub = self.create_publisher(
|
||||
TwistStamped,
|
||||
'/servo_node/delta_twist_cmds',
|
||||
10)
|
||||
|
||||
self.joint_pub = self.create_publisher(
|
||||
JointJog,
|
||||
'/servo_node/delta_joint_cmds',
|
||||
10)
|
||||
|
||||
self.gripper_cmd_pub = self.create_publisher(
|
||||
Float64MultiArray,
|
||||
'/forward_gripper_position_controller/commands',
|
||||
10)
|
||||
|
||||
# Services
|
||||
self.servo_client = self.create_client(Trigger, '/servo_node/start_servo')
|
||||
|
||||
srv_msg = Trigger.Request()
|
||||
while not self.servo_client.wait_for_service(timeout_sec=1.0):
|
||||
self.get_logger().info('/servo_node/start_servo service not available, waiting again...')
|
||||
|
||||
self.call_start_servo()
|
||||
|
||||
self.get_logger().info('kb_servo_node started!')
|
||||
|
||||
def call_start_servo(self):
|
||||
request = Trigger.Request()
|
||||
future = self.servo_client.call_async(request)
|
||||
rclpy.spin_until_future_complete(self, future)
|
||||
response = future.result()
|
||||
if response.success:
|
||||
self.get_logger().info(f'Successfully called start_servo: {response.message}')
|
||||
else:
|
||||
self.get_logger().info('Failed to call start_servo')
|
||||
|
||||
def joint_state_callback(self, msg):
|
||||
try:
|
||||
index = msg.name.index('finger_joint')
|
||||
self.last_finger_joint_angle = msg.position[index]
|
||||
except ValueError:
|
||||
self.get_logger().error('finger_joint not found in /joint_states msg')
|
||||
|
||||
def keyboard_callback(self, event):
|
||||
# Process keyboard events
|
||||
print("Key pressed")
|
||||
if event.event_type == keyboard.KEY_DOWN:
|
||||
# Handle key presses
|
||||
if event.name == 't':
|
||||
# Toggle mode between twist and joint control
|
||||
self.mode = 'joint' if self.mode == 'twist' else 'twist'
|
||||
self.get_logger().info(f'Mode switched to: {self.mode}')
|
||||
elif event.name == 'w':
|
||||
# Move forward
|
||||
self.publish_twist(1.0 * self.linear_speed_multiplier) # Adjust speed as needed
|
||||
self.get_logger().info('Moving forward')
|
||||
elif event.name == 's':
|
||||
# Move backward
|
||||
self.publish_twist(-1.0 * self.linear_speed_multiplier) # Adjust speed as needed
|
||||
self.get_logger().info('Moving backward')
|
||||
elif event.event_type == keyboard.KEY_UP:
|
||||
# Handle key releases
|
||||
if event.name == 'w' or event.name == 's':
|
||||
# Stop moving
|
||||
self.publish_twist(0.0)
|
||||
self.get_logger().info('Stopped moving')
|
||||
|
||||
def publish_twist(self, linear_speed):
|
||||
twist_msg = TwistStamped()
|
||||
twist_msg.header.frame_id = 'tool0'
|
||||
twist_msg.header.stamp = self.get_clock().now().to_msg()
|
||||
twist_msg.twist.linear.x = linear_speed
|
||||
twist_msg.twist.linear.y = 0.0 # Adjust as needed
|
||||
twist_msg.twist.linear.z = 0.0 # Adjust as needed
|
||||
twist_msg.twist.angular.x = 0.0
|
||||
twist_msg.twist.angular.y = 0.0
|
||||
twist_msg.twist.angular.z = 0.0
|
||||
self.twist_pub.publish(twist_msg)
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = KBControllerNode()
|
||||
rclpy.spin(node)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
Loading…
Reference in New Issue
Block a user