diff --git a/README.md b/README.md
index d2f87a2..61d3799 100644
--- a/README.md
+++ b/README.md
@@ -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
diff --git a/docker-compose.yaml b/docker-compose.yaml
index 0292553..c85f16f 100644
--- a/docker-compose.yaml
+++ b/docker-compose.yaml
@@ -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:
diff --git a/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml b/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml
index 9a50b20..6ecfce1 100644
--- a/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml
+++ b/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml
@@ -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 }
\ No newline at end of file
diff --git a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py
index b91cee9..c391a22 100644
--- a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py
+++ b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py
@@ -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",
diff --git a/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml b/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml
index 158f762..accd281 100644
--- a/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml
+++ b/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml
@@ -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
+
diff --git a/src/ur_robotiq_moveit_config/config/ur_servo.yaml b/src/ur_robotiq_moveit_config/config/ur_servo.yaml
index 9802702..b4362dc 100644
--- a/src/ur_robotiq_moveit_config/config/ur_servo.yaml
+++ b/src/ur_robotiq_moveit_config/config/ur_servo.yaml
@@ -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:
diff --git a/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py b/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py
index 61d39db..9018cf2 100644
--- a/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py
+++ b/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py
@@ -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
diff --git a/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro b/src/ur_robotiq_moveit_config/srdf/old_ur_robotiq_macro.srdf.xacro
similarity index 88%
rename from src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro
rename to src/ur_robotiq_moveit_config/srdf/old_ur_robotiq_macro.srdf.xacro
index 0f20db3..034a71d 100644
--- a/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro
+++ b/src/ur_robotiq_moveit_config/srdf/old_ur_robotiq_macro.srdf.xacro
@@ -64,6 +64,7 @@
+
@@ -102,6 +103,19 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -117,4 +131,4 @@
-
\ No newline at end of file
+
diff --git a/src/ur_robotiq_moveit_config/srdf/ur_manipulator_macro.srdf.xacro b/src/ur_robotiq_moveit_config/srdf/ur_manipulator_macro.srdf.xacro
new file mode 100644
index 0000000..92985df
--- /dev/null
+++ b/src/ur_robotiq_moveit_config/srdf/ur_manipulator_macro.srdf.xacro
@@ -0,0 +1,172 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/ur_robotiq_servo/config/joy-params.yaml b/src/ur_robotiq_servo/config/joy-params.yaml
new file mode 100644
index 0000000..8ad2d13
--- /dev/null
+++ b/src/ur_robotiq_servo/config/joy-params.yaml
@@ -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
\ No newline at end of file
diff --git a/src/ur_robotiq_servo/launch/ps5_servo.launch.py b/src/ur_robotiq_servo/launch/ps5_servo.launch.py
new file mode 100644
index 0000000..4ece31f
--- /dev/null
+++ b/src/ur_robotiq_servo/launch/ps5_servo.launch.py
@@ -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=[],
+ ),
+ ])
\ No newline at end of file
diff --git a/src/ur_robotiq_servo/package.xml b/src/ur_robotiq_servo/package.xml
index f936ee1..3099325 100644
--- a/src/ur_robotiq_servo/package.xml
+++ b/src/ur_robotiq_servo/package.xml
@@ -11,6 +11,7 @@
sensor_msgs
geometry_msgs
std_msgs
+ std_srvs
joy
moveit_servo
diff --git a/src/ur_robotiq_servo/setup.py b/src/ur_robotiq_servo/setup.py
index 1114fd8..d91e499 100644
--- a/src/ur_robotiq_servo/setup.py
+++ b/src/ur_robotiq_servo/setup.py
@@ -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'
],
},
)
diff --git a/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py b/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py
index e69de29..a19c3e7 100644
--- a/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py
+++ b/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py
@@ -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()
\ No newline at end of file