still error in moveit (gripper motion can be planned but not executed)

updated README.md

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

src/ur_robotiq_description/config/ur_robotiq_controllers.yaml

@@ -131,6 +131,7 @@ forward_position_controller:
       - $(var tf_prefix)wrist_2_joint
       - $(var tf_prefix)wrist_3_joint
 
+
 robotiq_gripper_controller:
   ros__parameters:
     default: true

src/ur_robotiq_description/launch/ur_robotiq_control.launch.py

@@ -7,6 +7,7 @@ from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.conditions import IfCondition, UnlessCondition
 from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, TextSubstitution
 
+
 def launch_setup(context, *args, **kwargs):
     # Initialize Arguments
     ur_type = LaunchConfiguration("ur_type")
@@ -44,8 +45,6 @@ def launch_setup(context, *args, **kwargs):
     script_sender_port = LaunchConfiguration("script_sender_port")
     trajectory_port = LaunchConfiguration("trajectory_port")
 
-
-
     # Config Paths UR
     joint_limit_params = PathJoinSubstitution(
         [FindPackageShare(ur_description_package), "config", ur_type, "joint_limits.yaml"]
@@ -191,8 +190,7 @@ def launch_setup(context, *args, **kwargs):
     control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
-        parameters=[
+        parameters=[robot_description,
-            robot_description,
                     update_rate_config_file,
                     ParameterFile(initial_joint_controllers, allow_substs=True),
                     ],
@@ -203,8 +201,7 @@ def launch_setup(context, *args, **kwargs):
     ur_control_node = Node(
         package="ur_robot_driver",
         executable="ur_ros2_control_node",
-        parameters=[
+        parameters=[robot_description,
-            robot_description,
                     update_rate_config_file,
                     ParameterFile(initial_joint_controllers, allow_substs=True),
                     ],
@@ -606,5 +603,4 @@ def generate_launch_description():
         )
     )
 
-
     return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

src/ur_robotiq_moveit_config/config/moveit_controllers.yaml

@@ -34,7 +34,7 @@ robotiq_gripper_controller:
   action_ns: gripper_cmd
   type: GripperCommand
   default: true
-  joints:
+  controlled_joints:
     - finger_joint
 
 

src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py

@@ -24,7 +24,7 @@ def load_yaml(package_name, file_path):
         return None
 
 
-def generate_launch_description():
+def launch_setup(context, *args, **kwargs):
 
     # Initialize Arguments
     ur_type = LaunchConfiguration("ur_type")
@@ -145,10 +145,12 @@ def generate_launch_description():
     ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
 
     # Trajectory Execution Configuration
-    controllers_yaml = load_yaml(moveit_config_package, "config/moveit_controllers.yaml")
     # the scaled_joint_trajectory_controller does not work on fake hardware
-    change_controllers = use_fake_hardware
+    use_fake_hw = use_fake_hardware.perform(context)
-    if change_controllers == "true":
+    controllers_yaml = load_yaml(moveit_config_package, "config/moveit_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
 
@@ -232,6 +234,12 @@ def generate_launch_description():
         output="screen",
     )
 
+    nodes_to_start = [move_group_node, rviz_node, servo_node]
+
+    return nodes_to_start
+
+
+def generate_launch_description():
     # Declare the Launch arguments
     declared_arguments = []
     # UR specific arguments
@@ -302,7 +310,6 @@ def generate_launch_description():
         DeclareLaunchArgument("launch_servo", default_value="true", description="Launch Servo?")
     )
 
-    # servo_node,
-    nodes_to_start = [move_group_node, rviz_node, servo_node]
 
-    return LaunchDescription(declared_arguments + nodes_to_start)
+
+    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

src/ur_robotiq_moveit_config/package.xml

@@ -50,6 +50,9 @@
   <exec_depend>robotiq_controllers</exec_depend>
   <exec_depend>gripper_controllers</exec_depend>
 
+  <depend>control_msgs</depend>
+  <depend>rclcpp_action</depend>
+
 
   <export>
       <build_type>ament_cmake</build_type>