Minor fixs

ur5_cell_moveit_config/config/initial_positions.yaml

@@ -1,9 +1,9 @@
 # Default initial positions for ur5_msa's ros2_control fake system
 
 initial_positions:
-  elbow_joint: 0
-  shoulder_lift_joint: 0
-  shoulder_pan_joint: 0
-  wrist_1_joint: 0
-  wrist_2_joint: 0
-  wrist_3_joint: 0
+  elbow_joint: -1.5794
+  shoulder_lift_joint: -1.74
+  shoulder_pan_joint: -1.5708
+  wrist_1_joint: 0.1736
+  wrist_2_joint: 1.5621
+  wrist_3_joint: 0

ur5_cell_moveit_config/config/ur5_msa.ros2_control.xacro

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:macro name="ur5_msa_ros2_control" params="name initial_positions_file">
-        <xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)['initial_positions']}"/>
+        <xacro:property name="initial_positions" value="${xacro.load_yaml(initial_positions_file)['initial_positions']}"/>
 
         <ros2_control name="${name}" type="system">
             <hardware>

ur5_cell_moveit_config/config/ur5_msa.srdf

@@ -18,6 +18,31 @@
         <joint name="shoulder_pan_joint"/>
         <chain base_link="base_link" tip_link="tool0"/>
     </group>
+    <group name="hand">
+        <joint name="shunk_egp50_tip_joint"/>
+        <joint name="shunk_egp50_pincer_2_joint"/>
+        <joint name="shunk_egp50_pincer_1_joint"/>
+        <joint name="shunk_egp50_body_joint"/>
+        <chain base_link="shunk_egp50_base_link" tip_link="tool_tip"/>
+    </group>
+    <group_state name="home" group="arm">
+        <joint name="elbow_joint" value="-1.5794" />
+        <joint name="shoulder_lift_joint" value="-1.74" />
+        <joint name="shoulder_pan_joint" value="-1.5708" />
+        <joint name="wrist_1_joint" value="0.1736" />
+        <joint name="wrist_2_joint" value="1.5621" />
+        <joint name="wrist_3_joint" value="0" />
+    </group_state>
+    <group_state name="detect" group="arm">
+        <joint name="elbow_joint" value="-1.5708" />
+        <joint name="shoulder_lift_joint" value="-1.5708" />
+        <joint name="shoulder_pan_joint" value="-1.5708" />
+        <joint name="wrist_1_joint" value="-1.5708" />
+        <joint name="wrist_2_joint" value="1.5708" />
+        <joint name="wrist_3_joint" value="0" />
+    </group_state>
+    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
+    <end_effector name="hand" parent_link="tool0" group="hand" parent_group="arm"/>
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <disable_collisions link1="base_link_inertia" link2="robotcell_base_link" reason="Adjacent"/>
     <disable_collisions link1="base_link_inertia" link2="shield_front_link" reason="Never"/>

ur5_cell_moveit_config/launch/robot.launch.py

@@ -10,11 +10,16 @@
 from moveit_configs_utils.launch_utils import DeclareBooleanLaunchArg
 from launch.substitutions import LaunchConfiguration
 from ament_index_python.packages import get_package_share_directory
+import os
+
 
 def generate_launch_description():
 
-    moveit_config = MoveItConfigsBuilder("ur5_msa", package_name="ur5_cell_moveit_config").to_moveit_configs()
-    #pprint.pprint(moveit_config.to_dict())
+    moveit_config = MoveItConfigsBuilder(
+        "ur5_msa", package_name="ur5_cell_moveit_config").to_moveit_configs()
+    # pprint.pprint(moveit_config.to_dict())
+    ROBOT_IP = os.environ['ROBOT_IP']
+
     ld = LaunchDescription()
     ld.add_action(
         DeclareBooleanLaunchArg(
@@ -32,15 +37,14 @@ def generate_launch_description():
     )
     ld.add_action(
         DeclareBooleanLaunchArg(
-            "use_rviz", 
+            "use_rviz",
             default_value=True)
-        )
-
+    )
 
     virtual_joints_launch = (
         moveit_config.package_path / "launch/static_virtual_joint_tfs.launch.py"
     )
-    
+
     if virtual_joints_launch.exists():
         ld.add_action(
             IncludeLaunchDescription(
@@ -87,12 +91,12 @@ def generate_launch_description():
             package="ur_robot_driver",
             executable="ur_ros2_control_node",
             parameters=[
-                moveit_config.robot_description, 
+                moveit_config.robot_description,
                 str(moveit_config.package_path / "config/ros2_controllers.yaml")],
             output="screen"
         )
-    )    
-    
+    )
+
     ld.add_action(
         Node(
             package="ur_robot_driver",
@@ -101,10 +105,10 @@ def generate_launch_description():
             name="dashboard_client",
             output="screen",
             emulate_tty=True,
-            parameters=[{"robot_ip": "192.168.56.2"}],
+            parameters=[{"robot_ip": ROBOT_IP}],
         )
     )
-    
+
     ld.add_action(
         Node(
             package="ur_robot_driver",
@@ -131,9 +135,10 @@ def generate_launch_description():
     ld.add_action(
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
-                str(moveit_config.package_path / "launch/spawn_controllers.launch.py")
+                str(moveit_config.package_path /
+                    "launch/spawn_controllers.launch.py")
             ),
         )
     )
-    
-    return ld
+
+    return ld

ur5_cell_moveit_config/package.xml

@@ -40,9 +40,10 @@
   <exec_depend>rviz2</exec_depend>
   <exec_depend>rviz_common</exec_depend>
   <exec_depend>rviz_default_plugins</exec_depend>
-  <exec_depend>tf2_ros</exec_depend>
   <exec_depend>ur_description</exec_depend>
   <exec_depend>xacro</exec_depend>
+  <exec_depend>ur5e_cell_description</exec_depend>
+  <exec_depend version_eq="2.2.6">ur_robot_driver</exec_depend>
 
 
   <export>

ur5e_cell_description/CMakeLists.txt

@@ -7,24 +7,23 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rclpy REQUIRED)
-find_package(ur_description)
 
 # add this line to include launch and urdf directory
-install(DIRECTORY 
-  launch 
-  urdf 
+install(DIRECTORY
+  launch
+  urdf
   config
   meshes
   rviz
   DESTINATION share/${PROJECT_NAME}/)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
+
   # the following line skips the linter which checks for copyrights
   # comment the line when a copyright and license is added to all source files
   set(ament_cmake_copyright_FOUND TRUE)
+
   # the following line skips cpplint (only works in a git repo)
   # comment the line when this package is in a git repo and when
   # a copyright and license is added to all source files

ur5e_cell_description/package.xml

@@ -9,12 +9,11 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <depend>rclcpp</depend>
-  <depend>rclpy</depend>
-  <depend>ur_description</depend>
-  <depend>ur_moveit_config</depend>
-  <depend>ur_robot_driver</depend>
+  <exec_depend>ur_description</exec_depend>
   <exec_depend>ros2launch</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
   <test_depend>ament_lint_auto</test_depend>
 
   <export>

ur5e_cell_description/urdf/workcell.urdf.xacro

@@ -61,7 +61,7 @@
         sim_gazebo="$(arg sim_gazebo)"
         sim_ignition="$(arg sim_ignition)"
         headless_mode="$(arg headless_mode)"
-        initial_positions="${load_yaml(initial_positions_file)}"
+        initial_positions="${xacro.load_yaml(initial_positions_file)}"
         use_tool_communication="$(arg use_tool_communication)"
         tool_voltage="$(arg tool_voltage)"
         tool_parity="$(arg tool_parity)"
@@ -103,4 +103,4 @@
     </joint>
 
 
-</robot>
+</robot>