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

gripper controlled by ps5 controller

Browse Source
This commit is contained in:
Niko Feith 2024-04-05 13:13:08 +02:00
parent 5d45c4ee6e
commit 149e44c90b
6 changed files with 69 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
README.md
View File

@ -90,4 +90,5 @@ Terminal 4:
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
```

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

@ -24,6 +24,9 @@ controller_manager:
forward_position_controller:
type: position_controllers/JointGroupPositionController
forward_gripper_position_controller:
type: position_controllers/JointGroupPositionController
robotiq_gripper_controller:
type: position_controllers/GripperActionController
@ -33,10 +36,6 @@ controller_manager:
robotiq_activation_controller:
type: robotiq_controllers/RobotiqActivationController
combined_joint_trajectory_controller:
type: joint_trajectory_controller/JointTrajectoryController
speed_scaling_state_broadcaster:
ros__parameters:
state_publish_rate: 100.0
@ -137,6 +136,11 @@ forward_position_controller:
- $(var tf_prefix)wrist_2_joint
- $(var tf_prefix)wrist_3_joint
forward_gripper_position_controller:
ros__parameters:
joints:
- $(var tf_prefix)finger_joint
robotiq_gripper_controller:
ros__parameters:
@ -163,33 +167,4 @@ robotiq_gripper_joint_trajectory_controller:
robotiq_activation_controller:
ros__parameters:
default: true
combined_joint_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 }
default: true

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

@ -271,8 +271,6 @@ def launch_setup(context, *args, **kwargs):
"force_torque_sensor_broadcaster",
"joint_state_broadcaster",
"speed_scaling_state_broadcaster",
"robotiq_gripper_controller",
"robotiq_activation_controller",
]
},
],
@ -306,8 +304,7 @@ def launch_setup(context, *args, **kwargs):
"/controller_manager",
"--controller-manager-timeout",
controller_spawner_timeout,
]
+ inactive_flags,
] + inactive_flags,
)
controller_spawner_names = [
@ -316,7 +313,8 @@ def launch_setup(context, *args, **kwargs):
"speed_scaling_state_broadcaster",
"force_torque_sensor_broadcaster",
]
controller_spawner_inactive_names = ["forward_position_controller", "combined_joint_trajectory_controller"]
controller_spawner_inactive_names = ["forward_position_controller",
"forward_gripper_position_controller"]
controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [
controller_spawner(name, active=False) for name in controller_spawner_inactive_names

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

@ -45,17 +45,3 @@ robotiq_gripper_joint_trajectory_controller:
default: false
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

5
src/ur_robotiq_servo/config/ps5-params.yaml
View File

@ -1,3 +1,6 @@
ps5_servo_node:
ros__parameters:
linear_speed_multiplier: 0.2
linear_speed_multiplier: 0.2
gripper_speed_multiplier: 0.02
gripper_lower_limit: 0.0
gripper_upper_limit: 0.70

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

@ -1,9 +1,10 @@
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Joy
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
class PS5ControllerNode(Node):
@ -12,6 +13,7 @@ class PS5ControllerNode(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)
@ -22,7 +24,27 @@ class PS5ControllerNode(Node):
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',
@ -39,6 +61,11 @@ class PS5ControllerNode(Node):
'/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')
@ -60,6 +87,13 @@ class PS5ControllerNode(Node):
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 joy_callback(self, msg):
# Check for button press to toggle mode
# Assuming button 2 (e.g., Triangle on PS5) for toggling mode
@ -104,6 +138,23 @@ class PS5ControllerNode(Node):
(msg.buttons[13] - msg.buttons[14]) * 1.0]
self.joint_pub.publish(joint_msg)
# Gripper controller
new_finger_joint_angle = (self.last_finger_joint_angle +
(msg.buttons[1] - msg.buttons[0]) * self.gripper_speed_multiplier)
if self.gripper_lower_limit > new_finger_joint_angle or self.gripper_upper_limit < new_finger_joint_angle:
self.get_logger().debug(f"New finger joint angle out of bounds: {new_finger_joint_angle}")
new_finger_joint_angle = self.last_finger_joint_angle
gripper_msg = Float64MultiArray()
layout_msg = MultiArrayLayout()
layout_msg.dim = [MultiArrayDimension()]
layout_msg.dim[0].label = "finger_joint"
layout_msg.dim[0].size = 1
layout_msg.dim[0].stride = 1
layout_msg.data_offset = 0
gripper_msg.layout = layout_msg
gripper_msg.data = [new_finger_joint_angle]
self.gripper_cmd_pub.publish(gripper_msg)
def main(args=None):
rclpy.init(args=args)