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

added ps5_control.py

Browse Source
This commit is contained in:
Niko Feith 2024-04-03 16:50:26 +02:00
parent 2fa4387147
commit 082616eb7e
14 changed files with 391 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
README.md
View File

@ -64,7 +64,7 @@ ros2 launch ur_robotiq_moveit_config ur_robotiq_moveit.launch.py use_fake_hardwa
### use_fake_hardware:=true
Terminal 1.:
```bash
ros2 launch ur_robotiq_description ur_robotiq_control.launch.py robot_ip:=aaa.bbb.ccc.ddd use_fake_hardware:=true launch_rviz:=false
ros2 launch ur_robotiq_description ur_robotiq_control.launch.py robot_ip:=aaa.bbb.ccc.ddd use_fake_hardware:=true launch_rviz:=false initial_joint_controller:=joint_trajectory_controller
```
Terminal 2.:
```bash

4
docker-compose.yaml
View File

@ -2,6 +2,7 @@ version: '3.8'
services:
ros2:
build: .
privileged: true
volumes:
- ./src:/ros2_ws/src
- /tmp/.X11-unix:/tmp/.X11-unix
@ -13,7 +14,8 @@ services:
- DISPLAY=$DISPLAY
devices:
- /dev/dri:/dev/dri
- /dev/input:/dev/input
- /dev/input/js0:/dev/input/js0
- /dev/input/js1:/dev/input/js1
networks:
ros_network:

31
src/ur_robotiq_description/config/ur_robotiq_controllers.yaml
View File

@ -33,6 +33,9 @@ controller_manager:
robotiq_activation_controller:
type: robotiq_controllers/RobotiqActivationController
combined_joint_trajectory_controller:
type: joint_trajectory_controller/JointTrajectoryController
speed_scaling_state_broadcaster:
ros__parameters:
@ -162,3 +165,31 @@ robotiq_activation_controller:
ros__parameters:
default: true
combined_trajectory_controller:
ros__parameters:
joints:
- $(var tf_prefix)shoulder_pan_joint
- $(var tf_prefix)shoulder_lift_joint
- $(var tf_prefix)elbow_joint
- $(var tf_prefix)wrist_1_joint
- $(var tf_prefix)wrist_2_joint
- $(var tf_prefix)wrist_3_joint
- $(var tf_prefix)finger_joint
command_interfaces:
- position
state_interfaces:
- position
- velocity
state_publish_rate: 100.0
action_monitor_rate: 20.0
allow_partial_joints_goal: false
constraints:
stopped_velocity_tolerance: 0.2
goal_time: 0.0
$(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
$(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
$(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 }
$(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
$(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
$(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
$(var tf_prefix)finger_joint: { trajectory: 0.2, goal: 0.1 }

17
src/ur_robotiq_description/launch/ur_robotiq_control.launch.py
View File

@ -25,6 +25,7 @@ def launch_setup(context, *args, **kwargs):
use_fake_hardware = LaunchConfiguration("use_fake_hardware")
fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
controller_spawner_timeout = LaunchConfiguration("controller_spawner_timeout")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
activate_joint_controller = LaunchConfiguration("activate_joint_controller")
launch_rviz = LaunchConfiguration("launch_rviz")
headless_mode = LaunchConfiguration("headless_mode")
@ -169,15 +170,11 @@ def launch_setup(context, *args, **kwargs):
robot_description = {"robot_description": robot_description_content}
if use_fake_hardware.perform(context):
initial_joint_controller = "joint_trajectory_controller"
else:
initial_joint_controller = "scaled_joint_trajectory_controller"
initial_joint_controllers = PathJoinSubstitution(
[FindPackageShare(description_package), "config", controllers_file]
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare(description_package), "rviz", "view_robot.rviz"]
)
@ -459,7 +456,7 @@ def generate_launch_description():
declared_arguments.append(
DeclareLaunchArgument(
"use_fake_hardware",
default_value="true",
default_value="false",
description="Start robot with fake hardware mirroring command to its states.",
)
)
@ -485,7 +482,13 @@ def generate_launch_description():
description="Timeout used when spawning controllers.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"initial_joint_controller",
default_value="scaled_joint_trajectory_controller",
description="Initially loaded robot controller.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"activate_joint_controller",

14
src/ur_robotiq_moveit_config/config/moveit_controllers.yaml
View File

@ -4,6 +4,7 @@ controller_names:
- joint_trajectory_controller
- robotiq_gripper_controller
- robotiq_gripper_joint_trajectory_controller
- combined_joint_trajectory_controller
scaled_joint_trajectory_controller:
@ -45,3 +46,16 @@ robotiq_gripper_joint_trajectory_controller:
joints:
- finger_joint
combined_joint_trajectory_controller:
type: FollowJointTrajectory
action_ns: follow_joint_trajectory
default: true
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
- finger_joint

2
src/ur_robotiq_moveit_config/config/ur_servo.yaml
View File

@ -2,7 +2,7 @@
# Modify all parameters related to servoing here
###############################################
use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
use_intra_process_comms : true
## Properties of incoming commands
command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
scale:

2
src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py
View File

@ -148,10 +148,8 @@ def launch_setup(context, *args, **kwargs):
# the scaled_joint_trajectory_controller does not work on fake hardware
use_fake_hw = use_fake_hardware.perform(context)
controllers_yaml = load_yaml(moveit_config_package, "config/moveit_controllers.yaml")
print(controllers_yaml)
if use_fake_hw == "true":
print("Using fake hardware")
controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False
controllers_yaml["joint_trajectory_controller"]["default"] = True
controllers_yaml["robotiq_gripper_controller"]["default"] = False

16
src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro → src/ur_robotiq_moveit_config/srdf/old_ur_robotiq_macro.srdf.xacro
View File

@ -64,6 +64,7 @@
<passive_joint name="${prefix}right_inner_finger_joint" />
<!--DISABLE COLLISIONS - By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<!--UR-Robot-->
<disable_collisions link1="${prefix}base" link2="${prefix}base_link" reason="Adjacent" />
<disable_collisions link1="${prefix}base_link" link2="${prefix}base_link_inertia" reason="Adjacent" />
<disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}shoulder_link" reason="Adjacent" />
<disable_collisions link1="${prefix}tool0" link2="${prefix}wrist_1_link" reason="Never" />
@ -102,6 +103,19 @@
<disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_inner_finger_pad" reason="Default"/>
<!-- Weird Bug-->
<disable_collisions link1="${prefix}base" link2="${prefix}base" reason="Adjacent" />
<disable_collisions link1="${prefix}base_link" link2="${prefix}base_link" reason="Adjacent" />
<disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}base_link_inertia" reason="Adjacent" />
<disable_collisions link1="${prefix}shoulder_link" link2="${prefix}shoulder_link" reason="Adjacent" />
<disable_collisions link1="${prefix}upper_arm_link" link2="${prefix}upper_arm_link" reason="Adjacent" />
<disable_collisions link1="${prefix}forearm_link" link2="${prefix}forearm_link" reason="Adjacent" />
<disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_1_link" reason="Adjacent" />
<disable_collisions link1="${prefix}wrist_2_link" link2="${prefix}wrist_2_link" reason="Adjacent" />
<disable_collisions link1="${prefix}wrist_3_link" link2="${prefix}wrist_3_link" reason="Adjacent" />
<disable_collisions link1="${prefix}flange" link2="${prefix}flange" reason="Adjacent" />
<disable_collisions link1="${prefix}ft_frame" link2="${prefix}ft_frame" reason="Adjacent" />
<disable_collisions link1="${prefix}tool0" link2="${prefix}tool0" reason="Adjacent" />
<disable_collisions link1="${prefix}robotiq_base_link" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}robotiq_140_base_link" link2="robotiq_140_base_link" reason="Adjacent"/>
@ -117,4 +131,4 @@
<disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_inner_finger" reason="Adjacent"/>
<disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}right_inner_finger_pad" reason="Default"/>
</xacro:macro>
</robot>
</robot>

172
src/ur_robotiq_moveit_config/srdf/ur_manipulator_macro.srdf.xacro Normal file
View File

@ -0,0 +1,172 @@
<?xml version="1.0" encoding="UTF-8"?>
<!--This does not replace URDF, and is not an extension of URDF.
This is a format for representing semantic information about the robot structure.
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">
<xacro:macro name="ur_robotiq_srdf" params="name prefix">
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
<group name="${prefix}${name}_manipulator">
<chain base_link="${prefix}base_link" tip_link="${prefix}tool0"/>
</group>
<group name="${prefix}gripper">
<joint name="${prefix}robotiq_base_joint"/>
<joint name="${prefix}robotiq_140_base_joint"/>
<joint name="${prefix}finger_joint"/>
<joint name="${prefix}left_outer_finger_joint"/>
<joint name="${prefix}left_inner_finger_joint"/>
<joint name="${prefix}left_inner_finger_pad_joint"/>
<joint name="${prefix}left_inner_knuckle_joint"/>
<joint name="${prefix}right_inner_knuckle_joint"/>
<joint name="${prefix}right_outer_knuckle_joint"/>
<joint name="${prefix}right_outer_finger_joint"/>
<joint name="${prefix}right_inner_finger_joint"/>
<joint name="${prefix}right_inner_finger_pad_joint"/>
</group>
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
<group_state name="${prefix}close" group="${prefix}gripper">
<joint name="${prefix}finger_joint" value="0.7"/>
</group_state>
<group_state name="${prefix}open" group="${prefix}gripper">
<joint name="${prefix}finger_joint" value="0"/>
</group_state>
<group_state name="${prefix}home" group="${prefix}${name}_manipulator">
<joint name="${prefix}elbow_joint" value="0" />
<joint name="${prefix}shoulder_lift_joint" value="-1.5707" />
<joint name="${prefix}shoulder_pan_joint" value="0" />
<joint name="${prefix}wrist_1_joint" value="0" />
<joint name="${prefix}wrist_2_joint" value="0" />
<joint name="${prefix}wrist_3_joint" value="0" />
</group_state>
<group_state name="${prefix}up" group="${prefix}${name}_manipulator">
<joint name="${prefix}elbow_joint" value="0" />
<joint name="${prefix}shoulder_lift_joint" value="-1.5707" />
<joint name="${prefix}shoulder_pan_joint" value="0" />
<joint name="${prefix}wrist_1_joint" value="-1.5707" />
<joint name="${prefix}wrist_2_joint" value="0" />
<joint name="${prefix}wrist_3_joint" value="0" />
</group_state>
<group_state name="${prefix}test_configuration" group="${prefix}${name}_manipulator">
<joint name="${prefix}elbow_joint" value="1.4" />
<joint name="${prefix}shoulder_lift_joint" value="-1.62" />
<joint name="${prefix}shoulder_pan_joint" value="1.54" />
<joint name="${prefix}wrist_1_joint" value="-1.2" />
<joint name="${prefix}wrist_2_joint" value="-1.6" />
<joint name="${prefix}wrist_3_joint" value="-0.11" />
</group_state>
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<end_effector name="${prefix}gripper" parent_link="${prefix}tool0" group="${prefix}gripper" parent_group="${prefix}${name}_manipulator"/>
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<virtual_joint name="world_joint" type="fixed" parent_frame="world" child_link="${prefix}base_link"/>
<!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
<passive_joint name="${prefix}right_inner_finger_joint"/>
<passive_joint name="${prefix}right_outer_knuckle_joint"/>
<passive_joint name="${prefix}right_inner_knuckle_joint"/>
<passive_joint name="${prefix}left_inner_knuckle_joint"/>
<passive_joint name="${prefix}left_inner_finger_joint"/>
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}shoulder_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}upper_arm_link" reason="Never"/>
<disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}wrist_1_link" reason="Never"/>
<disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}wrist_2_link" reason="Never"/>
<disable_collisions link1="${prefix}forearm_link" link2="${prefix}upper_arm_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}forearm_link" link2="${prefix}wrist_1_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_finger_pad" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_knuckle" reason="Default"/>
<disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_outer_finger" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_outer_knuckle" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}right_inner_finger" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}right_inner_finger_pad" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}right_inner_knuckle" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}right_outer_finger" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}right_outer_knuckle" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}wrist_1_link" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}wrist_2_link" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}wrist_3_link" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}left_inner_knuckle" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}left_outer_finger" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}left_outer_knuckle" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_inner_finger" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_inner_knuckle" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_outer_finger" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_outer_knuckle" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}robotiq_140_base_link" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}wrist_1_link" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}wrist_2_link" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}wrist_3_link" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_outer_finger" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_outer_knuckle" reason="Default"/>
<disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}right_inner_finger" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}right_inner_finger_pad" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}right_inner_knuckle" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}right_outer_finger" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}right_outer_knuckle" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
<disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
<disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}left_outer_knuckle" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}right_inner_finger" reason="Never"/>
<disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}right_inner_finger_pad" reason="Never"/>
<disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}right_inner_knuckle" reason="Never"/>
<disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}right_outer_finger" reason="Never"/>
<disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}right_outer_knuckle" reason="Never"/>
<disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
<disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}wrist_1_link" reason="Never"/>
<disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}wrist_2_link" reason="Never"/>
<disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}wrist_3_link" reason="Never"/>
<disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}right_inner_finger" reason="Never"/>
<disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}right_inner_finger_pad" reason="Never"/>
<disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}right_inner_knuckle" reason="Never"/>
<disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}right_outer_finger" reason="Never"/>
<disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}right_outer_knuckle" reason="Never"/>
<disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
<disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
<disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
<disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_inner_finger_pad" reason="Adjacent"/>
<disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_inner_knuckle" reason="Default"/>
<disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_outer_finger" reason="Adjacent"/>
<disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_outer_knuckle" reason="Never"/>
<disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
<disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}wrist_1_link" reason="Never"/>
<disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}wrist_2_link" reason="Never"/>
<disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}wrist_3_link" reason="Never"/>
<disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}right_inner_knuckle" reason="Never"/>
<disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}right_outer_finger" reason="Never"/>
<disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}right_outer_knuckle" reason="Never"/>
<disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}robotiq_140_base_link" reason="Never"/>
<disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}wrist_1_link" reason="Never"/>
<disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}wrist_2_link" reason="Never"/>
<disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}wrist_3_link" reason="Never"/>
<disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}right_outer_finger" reason="Never"/>
<disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}right_outer_knuckle" reason="Default"/>
<disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
<disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
<disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
<disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}right_outer_knuckle" reason="Adjacent"/>
<disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
<disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}wrist_1_link" reason="Never"/>
<disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}wrist_2_link" reason="Never"/>
<disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}wrist_3_link" reason="Never"/>
<disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
<disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
<disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
<disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}wrist_1_link" reason="Never"/>
<disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}wrist_2_link" reason="Never"/>
<disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}wrist_3_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}shoulder_link" link2="${prefix}upper_arm_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}shoulder_link" link2="${prefix}wrist_1_link" reason="Never"/>
<disable_collisions link1="${prefix}shoulder_link" link2="${prefix}wrist_2_link" reason="Never"/>
<disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_2_link" reason="Adjacent"/>
<disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_3_link" reason="Never"/>
<disable_collisions link1="${prefix}wrist_2_link" link2="${prefix}wrist_3_link" reason="Adjacent"/>
</xacro:macro>
</robot>

8
src/ur_robotiq_servo/config/joy-params.yaml Normal file
View File

@ -0,0 +1,8 @@
joy_node:
ros__parameters:
device_id: 0
device_name: "PS5 Controller"
deadzone: 0.5
autorepeat_rate: 20.0
sticky_buttons: false
coalesce_interval_ms: 1

31
src/ur_robotiq_servo/launch/ps5_servo.launch.py Normal file
View File

@ -0,0 +1,31 @@
import os
from launch import LaunchDescription
from launch_ros.actions import Node
import ament_index_python.packages
def generate_launch_description():
config_directory = os.path.join(
ament_index_python.packages.get_package_share_directory('ur_robotiq_servo'),
'config')
params = os.path.join(config_directory, 'joy-params.yaml')
return LaunchDescription([
Node(
package='joy',
executable='joy_node',
name='joy_node',
output='screen',
parameters=[{
params,
}],
),
Node(
package='ur_robotiq_servo',
executable='ps5_servo',
name='ps5_servo_node',
output='screen',
parameters=[{
}],
arguments=[],
),
])

1
src/ur_robotiq_servo/package.xml
View File

@ -11,6 +11,7 @@
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>
<depend>joy</depend>
<depend>moveit_servo</depend>

5
src/ur_robotiq_servo/setup.py
View File

@ -1,4 +1,6 @@
from setuptools import find_packages, setup
import os
from glob import glob
package_name = 'ur_robotiq_servo'
@ -10,6 +12,8 @@ setup(
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
(os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
],
install_requires=['setuptools'],
zip_safe=True,
@ -20,6 +24,7 @@ setup(
tests_require=['pytest'],
entry_points={
'console_scripts': [
'ps5_servo = ur_robotiq_servo.ps5_control:main'
],
},
)

99
src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py
View File

@ -0,0 +1,99 @@
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Joy
from geometry_msgs.msg import TwistStamped
from control_msgs.msg import JointJog
from std_srvs.srv import Trigger
class PS5ControllerNode(Node):
def __init__(self):
super().__init__('ps5_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
# Subscriber and Publisher
self.joy_sub = self.create_subscription(
Joy,
'/joy',
self.joy_callback,
1)
self.twist_pub = self.create_publisher(
TwistStamped,
'/servo_node/delta_twist_cmds',
1)
self.joint_pub = self.create_publisher(
JointJog,
'/servo_node/delta_joint_cmds',
1)
# 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('ps5_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 joy_callback(self, msg):
# Check for button press to toggle mode
# Assuming button 2 (e.g., Triangle on PS5) for toggling mode
current_button_state = msg.buttons[16]
if current_button_state == 1 and self.last_button_state == 0:
self.mode = 'joint' if self.mode == 'twist' else 'twist'
self.get_logger().info(f'Mode switched to: {self.mode}')
self.last_button_state = current_button_state
left_trigger = (msg.axes[2] - 1) / - 2.0
right_trigger = (msg.axes[5] - 1) / - 2.0
# Process control input based on current mode
if self.mode == 'twist':
twist_msg = TwistStamped()
twist_msg.header.frame_id = 'tool0'
twist_msg.header.stamp = self.get_clock().now().to_msg()
twist_msg.twist.linear.x = msg.axes[0]
twist_msg.twist.linear.y = msg.axes[1]
twist_msg.twist.linear.z = left_trigger - right_trigger
self.twist_pub.publish(twist_msg)
elif self.mode == 'joint':
joint_msg = JointJog()
joint_msg.header.frame_id = 'tool0'
joint_msg.header.stamp = self.get_clock().now().to_msg()
joint_msg.joint_names = ["shoulder_pan_joint",
"shoulder_lift_joint",
"elbow_joint",
"wrist_1_joint",
"wrist_2_joint",
"wrist_3_joint"]
joint_msg.velocities = [msg.axes[0],
msg.axes[1],
msg.axes[2],
msg.axes[3],
(msg.buttons[11] - msg.buttons[12]) * 1.0,
(msg.buttons[13] - msg.buttons[14]) * 1.0]
self.joint_pub.publish(joint_msg)
def main(args=None):
rclpy.init(args=args)
node = PS5ControllerNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()