controller from ur_robotiq_control.launch.py starts up

ur_robotiq_moveit.launch.py cant find gripper_gap joint

.idea/vcs.xml

@@ -4,6 +4,5 @@
     <mapping directory="$PROJECT_DIR$" vcs="Git" />
     <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" />
   </component>
 </project>

src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro

@@ -12,7 +12,7 @@
         robot_port:=63352">
 
 
-                <xacro:property name="config_file" value="$(find robotiq_2f_description)/config/robotiq_2f_140_config.yaml" />
+        <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']}" />

src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">
 
-	<!-- parameters -->    
+	<!-- parameters -->
 	<xacro:arg name="name" default="ur_manipulator"/>
 	<!-- import main macro -->
 	<xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>

src/ur_robotiq_moveit_config/config/joint_limits.yaml

@@ -1,50 +1,90 @@
-# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+# Joints limits
-
+#
-# For beginners, we downscale velocity and acceleration limits.
+# Sources:
-# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
+#
-default_velocity_scaling_factor: 0.1
+#  - Universal Robots e-Series, User Manual, UR3e, Version 5.8
-default_acceleration_scaling_factor: 0.1
+#    https://s3-eu-west-1.amazonaws.com/ur-support-site/69043/99403_UR3e_User_Manual_en_Global.pdf
-
+#  - Support > Articles > UR articles > Max. joint torques
-# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+#    https://www.universal-robots.com/articles/ur-articles/max-joint-torques
-# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+#    retrieved: 2020-06-16, last modified: 2020-06-09
 joint_limits:
-  elbow_joint:
-    has_velocity_limits: true
-    max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
-  left_inner_knuckle_joint:
-    has_velocity_limits: true
-    max_velocity: 2
-    has_acceleration_limits: false
-    max_acceleration: 0
-  right_inner_knuckle_joint:
-    has_velocity_limits: true
-    max_velocity: 2
-    has_acceleration_limits: false
-    max_acceleration: 0
-  shoulder_lift_joint:
-    has_velocity_limits: true
-    max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
   shoulder_pan_joint:
-    has_velocity_limits: true
+    # acceleration limits are not publicly available
-    max_velocity: 3.1415926535897931
     has_acceleration_limits: false
-    max_acceleration: 0
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 56.0
+    max_position: !degrees  360.0
+    max_velocity: !degrees  180.0
+    min_position: !degrees -360.0
+  shoulder_lift_joint:
+    # acceleration limits are not publicly available
+    has_acceleration_limits: false
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 56.0
+    max_position: !degrees  360.0
+    max_velocity: !degrees  180.0
+    min_position: !degrees -360.0
+  elbow_joint:
+    # acceleration limits are not publicly available
+    has_acceleration_limits: false
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 28.0
+    # we artificially limit this joint to half its actual joint position limit
+    # to avoid (MoveIt/OMPL) planning problems, as due to the physical
+    # construction of the robot, it's impossible to rotate the 'elbow_joint'
+    # over more than approx +- 1 pi (the shoulder lift joint gets in the way).
+    #
+    # This leads to planning problems as the search space will be divided into
+    # two sections, with no connections from one to the other.
+    #
+    # Refer to https://github.com/ros-industrial/universal_robot/issues/265 for
+    # more information.
+    max_position: !degrees  180.0
+    max_velocity: !degrees  180.0
+    min_position: !degrees -180.0
   wrist_1_joint:
-    has_velocity_limits: true
+    # acceleration limits are not publicly available
-    max_velocity: 6.2831853071795862
     has_acceleration_limits: false
-    max_acceleration: 0
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 12.0
+    max_position: !degrees  360.0
+    max_velocity: !degrees  360.0
+    min_position: !degrees -360.0
   wrist_2_joint:
-    has_velocity_limits: true
+    # acceleration limits are not publicly available
-    max_velocity: 6.2831853071795862
     has_acceleration_limits: false
-    max_acceleration: 0
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 12.0
+    max_position: !degrees  360.0
+    max_velocity: !degrees  360.0
+    min_position: !degrees -360.0
   wrist_3_joint:
-    has_velocity_limits: true
+    # acceleration limits are not publicly available
-    max_velocity: 6.2831853071795862
     has_acceleration_limits: false
-    max_acceleration: 0
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 12.0
+    max_position: !degrees  360.0
+    max_velocity: !degrees  360.0
+    min_position: !degrees -360.0
+  gripper_gap:
+    # acceleration limits are not publicly available
+    has_acceleration_limits: false
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 235.0
+    max_position: 0.14
+    max_velocity: 0.15
+    min_position: 0.0

src/ur_robotiq_moveit_config/config/moveit_controllers.yaml

@@ -1,4 +1,3 @@
-
 controller_names:
   - scaled_joint_trajectory_controller
   - joint_trajectory_controller

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
+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:

src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py

@@ -40,17 +40,17 @@ def launch_setup(context, *args, **kwargs):
     launch_servo = LaunchConfiguration("launch_servo")
 
     # File and package names
-    ur_description_package = "ur_description"
+    ur_description_package = LaunchConfiguration("ur_description_package")
-    ur_robotiq_description_package = "ur_robotiq_description"
+    ur_robotiq_description_package = LaunchConfiguration("ur_robotiq_description_package")
-    ur_robotiq_description_file = "ur_robotiq.urdf.xacro"
+    ur_robotiq_description_file = LaunchConfiguration("ur_robotiq_description_file")
-    moveit_config_package = "ur_robotiq_moveit_config"
+    moveit_config_package = LaunchConfiguration("moveit_config_package")
-    moveit_config_file = "ur_robotiq.srdf.xacro"
+    moveit_config_file = LaunchConfiguration("moveit_config_file")
 
     joint_limit_params = PathJoinSubstitution(
         [FindPackageShare(ur_description_package), "config", ur_type, "joint_limits.yaml"]
     )
     kinematics_params = PathJoinSubstitution(
-        [FindPackageShare(ur_description_package), "config", ur_type, "default_kinematics.yaml"]
+        [FindPackageShare(ur_robotiq_description_package), "config", "ur_robotiq_calibration.yaml"]
     )
     physical_params = PathJoinSubstitution(
         [FindPackageShare(ur_description_package), "config", ur_type, "physical_parameters.yaml"]
@@ -59,6 +59,8 @@ def launch_setup(context, *args, **kwargs):
         [FindPackageShare(ur_description_package), "config", ur_type, "visual_parameters.yaml"]
     )
 
+
+
     robot_description_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
@@ -126,7 +128,7 @@ def launch_setup(context, *args, **kwargs):
 
     robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
 
-    robot_description_kinematics_params = load_yaml(moveit_config_package, "config/kinematics.yaml")
+    robot_description_kinematics_params = load_yaml("ur_robotiq_moveit_config", "config/kinematics.yaml")
     robot_description_kinematics = {"robot_description_kinematics": robot_description_kinematics_params}
     # robot_description_planning = {
     #     "robot_description_planning": load_yaml(ur_moveit_config_package, "config/joint_limits.yaml")
@@ -140,16 +142,15 @@ def launch_setup(context, *args, **kwargs):
             "start_state_max_bounds_error": 0.1,
         }
     }
-
+    ompl_planning_yaml = load_yaml("ur_robotiq_moveit_config", "config/ompl_planning.yaml")
-    ompl_planning_yaml = load_yaml(moveit_config_package, "config/ompl_planning.yaml")
     ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
 
     # Trajectory Execution Configuration
     # 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")
+    controllers_yaml = load_yaml("ur_robotiq_moveit_config", "config/moveit_controllers.yaml")
 
-    if use_fake_hw == "true":
+    if use_fake_hw == "true" or use_fake_hw == "True":
         controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False
         controllers_yaml["joint_trajectory_controller"]["default"] = True
 
@@ -219,7 +220,7 @@ def launch_setup(context, *args, **kwargs):
 
     # Servo node for realtime control
 
-    servo_yaml = load_yaml(moveit_config_package, "config/ur_servo.yaml")
+    servo_yaml = load_yaml("ur_robotiq_moveit_config", "config/ur_servo.yaml")
     servo_params = {"moveit_servo": servo_yaml}
     servo_node = Node(
         package="moveit_servo",
@@ -241,7 +242,46 @@ def launch_setup(context, *args, **kwargs):
 def generate_launch_description():
     # Declare the Launch arguments
     declared_arguments = []
-    # UR specific arguments
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ur_description_package",
+            description="Description package with robot URDF/XACRO files. Usually the argument \
+        is not set, it enables use of a custom description.",
+            default_value="ur_description",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ur_robotiq_description_package",
+            description="Description package with robot and gripper URDF/XACRO files. Usually the argument \
+        is not set, it enables use of a custom description.",
+            default_value="ur_robotiq_description",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ur_robotiq_description_file",
+            description="URDF/XACRO description file with the robot.",
+            default_value="ur_robotiq.urdf.xacro",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "moveit_config_package",
+            description="MoveIt config package with robot and gripper SRDF/XACRO files. Usually the argument \
+        is not set, it enables use of a custom moveit config.",
+            default_value="ur_robotiq_moveit_config",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "moveit_config_file",
+            description="MoveIt SRDF/XACRO description file with the robot and gripper.",
+            default_value="ur_robotiq.srdf.xacro",
+        )
+    )
+
     declared_arguments.append(
         DeclareLaunchArgument(
             "ur_type",

src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro

@@ -30,10 +30,10 @@
         <joint name="${prefix}base_to_dummy"/>
     </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}open" group="${prefix}gripper">
+    <group_state name="open" group="${prefix}gripper">
         <joint name="${prefix}gripper_gap" value="0.14"/>
     </group_state>
-    <group_state name="${prefix}closed" group="${prefix}gripper">
+    <group_state name="closed" group="${prefix}gripper">
         <joint name="${prefix}gripper_gap" value="0.0"/>
     </group_state>
     <group_state name="${prefix}home" group="${prefix}${name}_manipulator">

src/ur_robotiq_servo/config/ps5-params.yaml

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

src/ur_robotiq_servo/package.xml

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

src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py

@@ -4,7 +4,7 @@ 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
+from robotiq_2f_msgs.msg import ForwardCommand
 
 
 class PS5ControllerNode(Node):
@@ -24,17 +24,17 @@ 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_position_multiplier = self.declare_parameter('gripper_position_multiplier', 1.0)
-        self.gripper_speed_multiplier = (self.get_parameter('gripper_speed_multiplier')
+        self.gripper_position_multiplier = (self.get_parameter('gripper_position_multiplier')
                                          .get_parameter_value().double_value)
-        self.get_logger().info(f"Gripper speed multiplier: {self.gripper_speed_multiplier}")
+        self.get_logger().info(f"Gripper position multiplier: {self.gripper_position_multiplier}")
 
-        self.gripper_lower_limit = self.declare_parameter('gripper_lower_limit', 1.0)
+        self.gripper_lower_limit = self.declare_parameter('gripper_lower_limit', 0.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.declare_parameter('gripper_upper_limit', 0.14)
         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}")
@@ -62,15 +62,15 @@ class PS5ControllerNode(Node):
             10)
 
         self.gripper_cmd_pub = self.create_publisher(
-            Float64MultiArray,
+            ForwardCommand,
-            '/forward_gripper_position_controller/commands',
+            '/forward_gripper_position_controller/command',
             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):
+        while not self.servo_client.wait_for_service(timeout_sec=5.0):
             self.get_logger().info('/servo_node/start_servo service not available, waiting again...')
 
         self.call_start_servo()
@@ -89,7 +89,7 @@ class PS5ControllerNode(Node):
 
     def joint_state_callback(self, msg):
         try:
-            index = msg.name.index('finger_joint')
+            index = msg.name.index('gripper_gap')
             self.last_finger_joint_angle = msg.position[index]
         except ValueError:
             self.get_logger().error('finger_joint not found in /joint_states msg')
@@ -140,19 +140,14 @@ class PS5ControllerNode(Node):
 
         # Gripper controller
         new_finger_joint_angle = (self.last_finger_joint_angle +
-                                  (msg.buttons[1] - msg.buttons[0]) * self.gripper_speed_multiplier)
+                                  (msg.buttons[1] - msg.buttons[0]) * self.gripper_position_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()
+        gripper_msg = ForwardCommand()
-        layout_msg = MultiArrayLayout()
+        gripper_msg.position = new_finger_joint_angle
-        layout_msg.dim = [MultiArrayDimension()]
+        gripper_msg.max_velocity = 0.15
-        layout_msg.dim[0].label = "finger_joint"
+        gripper_msg.max_effort = 235.0
-        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)