diff --git a/README.md b/README.md
index 68964df..3697e70 100644
--- a/README.md
+++ b/README.md
@@ -88,7 +88,7 @@ After launching you need to change the controller to enable the servo mode.\
Terminal 4:
```bash
ros2 control switch_controllers --deactivate joint_trajectory_controller
-ros2 control switch_controllers --deactivate robotiq_gripper_joint_trajectory_controller
+ros2 control switch_controllers --deactivate robotiq_gripper_controller
ros2 control switch_controllers --activate forward_position_controller
ros2 control switch_controllers --activate forward_gripper_position_controller
```
\ No newline at end of file
diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro
index c54f805..031e32d 100644
--- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro
+++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro
@@ -436,8 +436,8 @@
-
-
+
+
@@ -446,25 +446,23 @@
-
-
-
-
-
-
-
-
-
+
+
+
+
+
-
-
-
-
-
+
+
+
+
+
+
+
diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro
index afdd6fb..f55b6d1 100644
--- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro
+++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro
@@ -304,32 +304,5 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp
index b5e3878..c242270 100644
--- a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp
+++ b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp
@@ -510,7 +510,7 @@ double RobotiqGripperHardwareInterface::convertRawToAngle(int raw_position) {
if (std::isnan(max_angle_) || 0.0 >= max_angle_) {
throw std::runtime_error("Invalid gripper angle limits: max_angle is not configured correctly.");
}
- double scaled_angle = (static_cast(raw_position) / 255.0) * max_angle_;
+ double scaled_angle = ((230 - static_cast(raw_position)) / 227.0) * max_angle_;
return std::max(0.0, std::min(scaled_angle, max_angle_));
}
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 08e2ffb..9f43b31 100644
--- a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py
+++ b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py
@@ -217,13 +217,6 @@ def launch_setup(context, *args, **kwargs):
package="controller_manager",
executable="spawner",
arguments=["robotiq_gripper_controller", "-c", "/controller_manager"],
- condition=UnlessCondition(use_fake_hardware),
- )
- robotiq_gripper_joint_trajectory_controller_spawner = Node(
- package="controller_manager",
- executable="spawner",
- arguments=["robotiq_gripper_joint_trajectory_controller", "-c", "/controller_manager"],
- condition=IfCondition(use_fake_hardware),
)
robotiq_activation_controller_spawner = Node(
@@ -347,7 +340,6 @@ def launch_setup(context, *args, **kwargs):
initial_joint_controller_spawner_stopped,
initial_joint_controller_spawner_started,
robotiq_gripper_controller_spawner,
- robotiq_gripper_joint_trajectory_controller_spawner,
robotiq_activation_controller_spawner,
] + controller_spawners
diff --git a/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml b/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml
index 2cedcfc..6814368 100644
--- a/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml
+++ b/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml
@@ -31,8 +31,8 @@ joint_trajectory_controller:
robotiq_gripper_controller:
- action_ns: robotiq_2f_controllers
- type: RobotiqGripperCommandController
+ action_ns: gripper_cmd
+ type: GripperCommand
default: true
- joint: gripper_gap
- max_gap: 0.14
+ joints:
+ - gripper_gap
diff --git a/src/ur_robotiq_moveit_config/config/ur_servo.yaml b/src/ur_robotiq_moveit_config/config/ur_servo.yaml
index 75d5e2b..493ee9d 100644
--- a/src/ur_robotiq_moveit_config/config/ur_servo.yaml
+++ b/src/ur_robotiq_moveit_config/config/ur_servo.yaml
@@ -7,10 +7,10 @@ use_intra_process_comms : True
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:
# Scale parameters are only used if command_in_type=="unitless"
- linear: 0.00001 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
- rotational: 0.001 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
+ linear: 0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
+ rotational: 0.8 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
# Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
- joint: 0.01
+ joint: 0.5
# This is a fudge factor to account for any latency in the system, e.g. network latency or poor low-level
# controller performance. It essentially increases the timestep when calculating the target pose, to move the target
# pose farther away. [seconds]
@@ -38,6 +38,7 @@ low_pass_filter_coeff: 2.0 # Larger --> trust the filtered data more, trust the
## MoveIt properties
move_group_name: ur_manipulator # Often 'manipulator' or 'arm'
planning_frame: world # The MoveIt planning frame. Often 'base_link' or 'world'
+is_primary_planning_scene_monitor: false
## Other frames
ee_frame_name: tool0 # The name of the end effector link, used to return the EE pose
diff --git a/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro b/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro
index a4778c0..c4187e6 100644
--- a/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro
+++ b/src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro
@@ -27,14 +27,15 @@
-
+
+
diff --git a/src/ur_robotiq_servo/config/joy-params.yaml b/src/ur_robotiq_servo/config/joy-params.yaml
index 8ad2d13..3119f4f 100644
--- a/src/ur_robotiq_servo/config/joy-params.yaml
+++ b/src/ur_robotiq_servo/config/joy-params.yaml
@@ -1,7 +1,7 @@
joy_node:
ros__parameters:
device_id: 0
- device_name: "PS5 Controller"
+ device_name: "Wireless Controller"
deadzone: 0.5
autorepeat_rate: 20.0
sticky_buttons: false
diff --git a/src/ur_robotiq_servo/config/ps5-params.yaml b/src/ur_robotiq_servo/config/ps5-params.yaml
index 1cc29a7..01da6c4 100644
--- a/src/ur_robotiq_servo/config/ps5-params.yaml
+++ b/src/ur_robotiq_servo/config/ps5-params.yaml
@@ -1,6 +1,6 @@
ps5_servo_node:
ros__parameters:
- linear_speed_multiplier: 0.2
- gripper_position_multiplier: 0.02
+ linear_speed_multiplier: 0.5
+ gripper_position_multiplier: 0.004
gripper_lower_limit: 0.0
gripper_upper_limit: 0.14
\ No newline at end of file
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 639d9ad..da78597 100644
--- a/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py
+++ b/src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py
@@ -97,13 +97,13 @@ class PS5ControllerNode(Node):
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]
+ current_button_state = msg.buttons[8]
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[4] - 1) / - 2.0
+ left_trigger = (msg.axes[2] - 1) / - 2.0
right_trigger = (msg.axes[5] - 1) / - 2.0
# Process control input based on current mode
@@ -115,8 +115,8 @@ class PS5ControllerNode(Node):
twist_msg.twist.linear.y = -msg.axes[1] * self.linear_speed_multiplier
twist_msg.twist.linear.z = (left_trigger - right_trigger) * self.linear_speed_multiplier
twist_msg.twist.angular.x = msg.axes[3]
- twist_msg.twist.angular.y = msg.axes[2]
- twist_msg.twist.angular.z = (msg.buttons[9] - msg.buttons[10]) * 1.0
+ twist_msg.twist.angular.y = msg.axes[4]
+ twist_msg.twist.angular.z = (msg.buttons[4] - msg.buttons[5]) * 1.0
self.twist_pub.publish(twist_msg)
elif self.mode == 'joint':
joint_msg = JointJog()
@@ -132,10 +132,10 @@ class PS5ControllerNode(Node):
]
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]
+ msg.axes[4],
+ msg.axes[6],
+ msg.axes[7]]
self.joint_pub.publish(joint_msg)
# Gripper controller