improved iisy_config module

.gitignore

@@ -35,3 +35,4 @@ AMENT_IGNORE
 
 # End of https://www.toptal.com/developers/gitignore/api/ros2
 
+.vscode

src/gaz_simulation/CMakeLists.txt

@@ -13,8 +13,6 @@ find_package(ament_cmake REQUIRED)
 
 install(DIRECTORY   launch    DESTINATION   share/${PROJECT_NAME})
 install(DIRECTORY   world     DESTINATION   share/${PROJECT_NAME})
-install(DIRECTORY   urdf      DESTINATION   share/${PROJECT_NAME})
-install(DIRECTORY   stl       DESTINATION   share/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

src/gaz_simulation/launch/moveit_launch.py

@@ -4,7 +4,7 @@ from launch import LaunchDescription
 from launch_ros.actions import Node
 from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, UnsetEnvironmentVariable
 from launch_ros.substitutions import FindPackageShare
-from launch.substitutions import Command
+from launch.substitutions import Command, LaunchConfiguration
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from ament_index_python.packages import get_package_share_directory
 
@@ -118,18 +118,15 @@ def generate_launch_description():
             output='screen'
         ),
 
-        Node(
+        #Node(
-            package='gazebo_ros',
+        #    package="gazebo_ros",
-            executable='spawn_entity.py',
+        #    executable="spawn_entity.py",
-            arguments=['-entity', robot_name,
+        #    arguments=[
-                       '-topic', 'robot_description',
+        #        "-topic", "robot_description",
-                       '-x', '-1',
+        #        "-entity", LaunchConfiguration("iisy")
-                       '-y', '-1',
+        #    ],
-                       '-z', '1',
+        #    output="screen"
-                       '-Y', '0'],  # Yaw
+        #),
-            output='screen'
-        ),
-
 
         #Node(
         #    package='robot_state_publisher',
@@ -159,21 +156,30 @@ def generate_launch_description():
         #    arguments=["test.xml"]
         # )
 
+        Node(
+            package='controller_manager',
+            name='gazebo_controller_spawner',
+            executable='spawner',
+            output='screen',
+            parameters=[
+                controllers_yaml,
+            ]
+        ),
         
-        #Node(package='moveit_ros_move_group',
+        Node(package='moveit_ros_move_group',
-        #    executable='move_group',
+            executable='move_group',
-        #    #prefix='xterm -fs 10 -e gdb --ex run --args',
+            #prefix='xterm -fs 10 -e gdb --ex run --args',
-        #    output='screen',
+            output='screen',
-        #    parameters=[
+            parameters=[
-        #        robot_description,
+                robot_description,
-        #        robot_description_semantic,
+                robot_description_semantic,
-        #        kinematics_yaml,
+                kinematics_yaml,
-        #        ompl_planning_pipeline_config,
+                ompl_planning_pipeline_config,
-        #        trajectory_execution,
+                trajectory_execution,
-        #        moveit_controllers,
+                moveit_controllers,
-        #        planning_scene_monitor_parameters,
+                planning_scene_monitor_parameters,
-        #        #octomap_config,
+                #octomap_config,
-        #        #octomap_updater_config
+                #octomap_updater_config
-        #    ]
+            ]
-        #)
+        ),
     ])

src/iisy_config/config/config.rviz

@@ -0,0 +1,384 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 212
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        lbr_link_0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        lbr_link_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        lbr_link_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        lbr_link_3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        lbr_link_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        lbr_link_5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        lbr_link_6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        lbr_link_7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        lbr_link_ee:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Acceleration_Scaling_Factor: 0.1
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: true
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: false
+      MoveIt_Warehouse_Host: 127.0.0.1
+      MoveIt_Warehouse_Port: 33829
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
+      Name: MotionPlanning
+      Planned Path:
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          lbr_link_0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          lbr_link_1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          lbr_link_2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          lbr_link_3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          lbr_link_4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          lbr_link_5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          lbr_link_6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          lbr_link_7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          lbr_link_ee:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Loop Animation: false
+        Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
+        State Display Time: 0.05 s
+        Trail Step Size: 1
+        Trajectory Topic: /display_planned_path
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.07999999821186066
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 1
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 0
+        Joint Violation Color: 255; 0; 255
+        Planning Group: med7_arm
+        Query Goal State: true
+        Query Start State: false
+        Show Workspace: false
+        Start State Alpha: 1
+        Start State Color: 0; 255; 0
+      Planning Scene Topic: /monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          lbr_link_0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          lbr_link_1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          lbr_link_2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          lbr_link_3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          lbr_link_4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          lbr_link_5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          lbr_link_6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          lbr_link_7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          lbr_link_ee:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+      Velocity_Scaling_Factor: 0.01
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 2.398728847503662
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.056836556643247604
+        Y: -0.0032705869525671005
+        Z: 0.7173376083374023
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5103980898857117
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 5.248579978942871
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: true
+  MotionPlanning:
+    collapsed: false
+  MotionPlanning - Trajectory Slider:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f30000032afc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000111000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000154000002130000017d00ffffff000000010000010000000280fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000280000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000006efc0100000002fb0000000800540069006d00650100000000000007380000005800fffffffb0000000800540069006d006501000000000000045000000000000000000000053f0000032a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1848
+  X: 0
+  Y: 27

src/iisy_config/config/iisy_controllers.yml

@@ -0,0 +1,42 @@
+# see controllers: http://control.ros.org/ros2_controllers/doc/controllers_index.html
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster  # if left empty, all states are published https://github.com/ros-controls/ros2_controllers/blob/b9afbcd74a5263fafa77181187b8ffa8f0696ea8/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp#L41
+
+    position_trajectory_controller:
+      # find declaration here https://github.com/ros-controls/ros2_controllers/blob/b9afbcd74a5263fafa77181187b8ffa8f0696ea8/joint_trajectory_controller/joint_trajectory_plugin.xml#L2
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    forward_position_controller:
+      type: position_controllers/JointGroupPositionController
+
+position_trajectory_controller:
+  ros__parameters:
+    # find required parameters here https://github.com/ros-controls/ros2_controllers/blob/master/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+    joints:
+      - base_rot
+      - base_link1_joint
+      - link1_link2_joint
+      - link2_rot
+      - link2_link3_joint
+      - link3_rot
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+    state_publish_rate: 50.0
+    action_monitor_rate: 20.0
+
+forward_position_controller:
+  ros__parameters:
+    joints:
+      - base_rot
+      - base_link1_joint
+      - link1_link2_joint
+      - link2_rot
+      - link2_link3_joint
+      - link3_rot
+    interface_name: "position"

src/iisy_config/config/iisy_moveit_controllers.yml

@@ -0,0 +1,15 @@
+controller_names:
+  - position_trajectory_controller
+
+# http://control.ros.org/ros2_controllers/joint_trajectory_controller/doc/userdoc.html#ros2-interface-of-the-controller
+position_trajectory_controller:
+  action_ns: follow_joint_trajectory
+  type: FollowJointTrajectory
+  default: true
+  joints:
+    - base_rot
+    - base_link1_joint
+    - link1_link2_joint
+    - link2_rot
+    - link2_link3_joint
+    - link3_rot

src/iisy_config/launch/chomp_planning_pipeline.launch.xml

@@ -1,23 +0,0 @@
-<launch>
-  <arg name="start_state_max_bounds_error" default="0.1" />
-  <arg name="jiggle_fraction" default="0.05" />
-  <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
-  <arg name="planning_adapters"
-       default="default_planner_request_adapters/AddTimeParameterization
-                default_planner_request_adapters/ResolveConstraintFrames
-                default_planner_request_adapters/FixWorkspaceBounds
-                default_planner_request_adapters/FixStartStateBounds
-                default_planner_request_adapters/FixStartStateCollision
-                default_planner_request_adapters/FixStartStatePathConstraints"
-                />
-
-  <param name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
-  <param name="request_adapters" value="$(arg planning_adapters)" />
-  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
-  <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
-
-  <!-- Add MoveGroup capabilities specific to this pipeline -->
-  <!-- <param name="capabilities" value="" /> -->
-
-  <rosparam command="load" file="$(find iisy_config)/config/chomp_planning.yaml" />
-</launch>

src/iisy_config/launch/default_warehouse_db.launch

@@ -1,15 +0,0 @@
-<launch>
-
-  <arg name="reset" default="false"/>
-  <!-- If not specified, we'll use a default database location -->
-  <arg name="moveit_warehouse_database_path" default="$(find iisy_config)/default_warehouse_mongo_db" />
-
-  <!-- Launch the warehouse with the configured database location -->
-  <include file="$(dirname)/warehouse.launch">
-    <arg name="moveit_warehouse_database_path" value="$(arg moveit_warehouse_database_path)" />
-  </include>
-
-  <!-- If we want to reset the database, run this node -->
-  <node if="$(arg reset)" name="$(anon moveit_default_db_reset)" type="moveit_init_demo_warehouse" pkg="moveit_ros_warehouse" respawn="false" output="screen" />
-
-</launch>

src/iisy_config/launch/demo.launch

@@ -1,68 +0,0 @@
-<launch>
-
-  <!-- specify the planning pipeline -->
-  <arg name="pipeline" default="ompl" />
-
-  <!-- By default, we do not start a database (it can be large) -->
-  <arg name="db" default="false" />
-  <!-- Allow user to specify database location -->
-  <arg name="db_path" default="$(find iisy_config)/default_warehouse_mongo_db" />
-
-  <!-- By default, we are not in debug mode -->
-  <arg name="debug" default="false" />
-
-  <!-- By default, we will load or override the robot_description -->
-  <arg name="load_robot_description" default="true"/>
-
-  <!-- Choose controller manager: fake, simple, or ros_control -->
-  <arg name="moveit_controller_manager" default="fake" />
-  <!-- Set execution mode for fake execution controllers -->
-  <arg name="fake_execution_type" default="interpolate" />
-
-  <!-- By default, hide joint_state_publisher's GUI in 'fake' controller_manager mode -->
-  <arg name="use_gui" default="false" />
-  <arg name="use_rviz" default="true" />
-
-  <!-- If needed, broadcast static tf for robot root -->
-
-
-
-  <group if="$(eval arg('moveit_controller_manager') == 'fake')">
-    <!-- We do not have a real robot connected, so publish fake joint states via a joint_state_publisher
-         MoveIt's fake controller's joint states are considered via the 'source_list' parameter -->
-    <node name="joint_state_publisher" pkg="joint_state_publisher" exec="joint_state_publisher" unless="$(arg use_gui)">
-      <param from="source_list">[move_group/fake_controller_joint_states]</param>
-    </node>
-    <!-- If desired, a GUI version is available allowing to move the simulated robot around manually
-         This corresponds to moving around the real robot without the use of MoveIt. -->
-    <node name="joint_state_publisher" pkg="joint_state_publisher_gui" exec="joint_state_publisher_gui" if="$(arg use_gui)">
-      <param from="source_list">[move_group/fake_controller_joint_states]</param>
-    </node>
-
-    <!-- Given the published joint states, publish tf for the robot links -->
-    <node name="robot_state_publisher" pkg="robot_state_publisher" exec="robot_state_publisher" respawn="true" output="screen" />
-  </group>
-
-  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
-  <include file="$(dirname)/move_group.launch">
-    <let name="allow_trajectory_execution" value="true"/>
-    <let name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
-    <let name="fake_execution_type" value="$(arg fake_execution_type)"/>
-    <let name="info" value="true"/>
-    <let name="debug" value="$(arg debug)"/>
-    <let name="pipeline" value="$(arg pipeline)"/>
-    <let name="load_robot_description" value="$(arg load_robot_description)"/>
-  </include>
-
-  <!-- Run Rviz and load the default config to see the state of the move_group node -->
-  <include file="$(dirname)/moveit_rviz.launch" if="$(arg use_rviz)">
-    <let name="rviz_config" value="$(dirname)/moveit.rviz"/>
-    <let name="debug" value="$(arg debug)"/>
-  </include>
-
-  <!-- If database loading was enabled, start mongodb as well -->
-  <include file="$(dirname)/default_warehouse_db.launch" if="$(arg db)">
-    <let name="moveit_warehouse_database_path" value="$(arg db_path)"/>
-  </include>
-
-</launch>

src/iisy_config/launch/demo_gazebo.launch

@@ -1,21 +0,0 @@
-<launch>
-
-  <!-- specify the planning pipeline -->
-  <arg name="pipeline" default="ompl" />
-
-  <!-- Gazebo specific options -->
-  <arg name="gazebo_gui" default="true"/>
-  <arg name="paused" default="false"/>
-
-  <!-- launch the gazebo simulator and spawn the robot -->
-  <include file="$(dirname)/gazebo.launch" >
-    <arg name="paused" value="$(arg paused)"/>
-    <arg name="gazebo_gui" value="$(arg gazebo_gui)"/>
-  </include>
-
-  <include file="$(dirname)/demo.launch" pass_all_args="true">
-    <!-- robot description is loaded by gazebo.launch, to enable Gazebo features -->
-    <arg name="load_robot_description" value="false" />
-    <arg name="moveit_controller_manager" value="ros_control" />
-  </include>
-</launch>

src/iisy_config/launch/fake_moveit_controller_manager.launch.xml

@@ -1,12 +0,0 @@
-<launch>
-
-  <!-- execute the trajectory in 'interpolate' mode or jump to goal position in 'last point' mode -->
-  <arg name="fake_execution_type" default="interpolate" />
-
-  <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
-  <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
-
-  <!-- The rest of the params are specific to this plugin -->
-  <rosparam subst_value="true" file="$(find iisy_config)/config/fake_controllers.yaml"/>
-
-</launch>

src/iisy_config/launch/gazebo.launch

@@ -1,32 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-  <arg name="paused" default="false"/>
-  <arg name="gazebo_gui" default="true"/>
-  <arg name="initial_joint_positions" doc="Initial joint configuration of the robot"
-       default=" -J base_link1_joint 0 -J base_rot 0 -J link1_link2_joint 0 -J link2_link3_joint 0 -J link2_rot 0 -J link3_rot 0"/>
-
-  <!-- startup simulated world -->
-  <include file="$(find gazebo_ros)/launch/empty_world.launch">
-    <let name="world_name" default="worlds/empty.world"/>
-    <let name="paused" value="true"/>
-    <let name="gui" value="$(arg gazebo_gui)"/>
-  </include>
-
-  <!-- send robot urdf to param server -->
-  <param name="robot_description" textfile="$(find gaz_simulation)/urdf/iisy.urdf" />
-
-  <!-- unpause only after loading robot model -->
-  <let name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
-  <!-- push robot_description to factory and spawn robot in gazebo at the origin, change x,y,z arguments to spawn in a different position -->
-  <arg name="world_pose" value="-x 0 -y 0 -z 0" />
-  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot $(arg unpause) $(arg world_pose) $(arg initial_joint_positions)"
-    respawn="false" output="screen" />
-
-  <!-- Load joint controller parameters for Gazebo -->
-  <param from="$(find iisy_config)/config/gazebo_controllers.yaml" />
-  <!-- Spawn Gazebo ROS controllers -->
-  <node name="gazebo_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller" />
-  <!-- Load ROS controllers -->
-  <include file="$(dirname)/ros_controllers.launch"/>
-
-</launch>

src/iisy_config/launch/iisy_moveit_sensor_manager.launch.xml

@@ -1,3 +0,0 @@
-<launch>
-
-</launch>

src/iisy_config/launch/joystick_control.launch

@@ -1,17 +0,0 @@
-<launch>
-  <!-- See moveit_ros/visualization/doc/joystick.rst for documentation -->
-
-  <arg name="dev" default="/dev/input/js0" />
-
-  <!-- Launch joy node -->
-  <node pkg="joy" type="joy_node" name="joy">
-    <param name="dev" value="$(arg dev)" /> <!-- Customize this to match the location your joystick is plugged in on-->
-    <param name="deadzone" value="0.2" />
-    <param name="autorepeat_rate" value="40" />
-    <param name="coalesce_interval" value="0.025" />
-  </node>
-
-  <!-- Launch python interface -->
-  <node pkg="moveit_ros_visualization" type="moveit_joy.py" output="screen" name="moveit_joy"/>
-
-</launch>

src/iisy_config/launch/lbr_bringup.launch.py

@@ -0,0 +1,152 @@
+from launch.actions.declare_launch_argument import DeclareLaunchArgument
+from launch.launch_description import LaunchDescription
+from launch.actions import IncludeLaunchDescription, OpaqueFunction
+from launch.conditions import IfCondition
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+from launch.substitutions.launch_configuration import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def launch_setup(context, *args, **kwargs):
+
+    # Evaluate frequently used variables
+    model = LaunchConfiguration("model").perform(context)
+
+    # Load robot description
+    robot_description_content = Command(
+        [
+            FindExecutable(name="xacro"), " ",
+            PathJoinSubstitution(
+                [FindPackageShare("iisy_config"), "urdf/{}.urdf".format( model)]
+            ), " ",
+            "robot_name:=", LaunchConfiguration("robot_name"), " ",
+            "sim:=", LaunchConfiguration("sim")
+        ]
+    )
+
+    # Load controls
+    control = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare("iisy_config"),
+                "launch",
+                "lbr_control.launch.py"
+            ])
+        ), launch_arguments=[
+            ("robot_description", robot_description_content),
+            ("controller_configurations_package", LaunchConfiguration("controller_configurations_package")),
+            ("controller_configurations", LaunchConfiguration("controller_configurations")),
+            ("controller", LaunchConfiguration("controller")),
+            ("sim", LaunchConfiguration("sim"))
+        ]
+    )
+
+    # Gazebo simulation 
+    simulation = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare("iisy_config"),
+                "launch",
+                "lbr_simulation.launch.py"
+            ])
+        ), 
+        launch_arguments=[
+           ("robot_name", LaunchConfiguration("robot_name"))
+        ],
+        condition=IfCondition(LaunchConfiguration("sim"))
+    )
+
+    # Move group
+    move_group = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare("iisy_config"),
+                "launch",
+                "lbr_move_group.launch.py"
+            ])
+        ), 
+        launch_arguments=[
+            ("robot_description", robot_description_content),
+            ("moveit_controller_configurations_package", LaunchConfiguration("moveit_controller_configurations_package")),
+            ("moveit_controller_configurations", LaunchConfiguration("moveit_controller_configurations")),
+            ("model", LaunchConfiguration("model")),
+            ("sim", LaunchConfiguration("sim"))
+        ]
+    )
+
+    return [
+        simulation,
+        control,
+        move_group
+    ]
+
+
+def generate_launch_description():
+
+    # Launch arguments
+    launch_args = []
+
+    launch_args.append(DeclareLaunchArgument(
+        name="model",
+        default_value="iisy",
+        description="Desired LBR model. Use model:=iiwa7/iiwa14/med7/med14."
+    ))
+
+    launch_args.append(DeclareLaunchArgument(
+        name="robot_name",
+        default_value="iisy",
+        description="Set robot name."
+    ))
+
+    launch_args.append(DeclareLaunchArgument(
+        name="sim",
+        default_value="true",
+        description="Launch robot in simulation or on real setup."
+    ))
+
+    launch_args.append(
+        DeclareLaunchArgument(
+            name="controller_configurations_package",
+            default_value="iisy_config",
+            description="Package that contains controller configurations."
+        )
+    )
+
+    launch_args.append(
+        DeclareLaunchArgument(
+            name="controller_configurations",
+            default_value="config/iisy_controllers.yml",
+            description="Relative path to controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name."
+        )
+    )
+
+    launch_args.append(
+        DeclareLaunchArgument(
+            name="controller",
+            default_value="position_trajectory_controller",
+            description="Robot controller."
+        )
+    )
+
+    launch_args.append(
+        DeclareLaunchArgument(
+            name="moveit_controller_configurations_package",
+            default_value="iisy_config",
+            description="Package that contains MoveIt! controller configurations."
+        )
+    )
+
+    launch_args.append(
+        DeclareLaunchArgument(
+            name="moveit_controller_configurations",
+            default_value="config/iisy_moveit_controllers.yml",
+            description="Relative path to MoveIt! controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name. This file lists controllers that are loaded through the controller_configurations file."
+        )
+    )
+
+    return LaunchDescription(
+        launch_args + [
+        OpaqueFunction(function=launch_setup)
+    ])

src/iisy_config/launch/lbr_control.launch.py

@@ -0,0 +1,86 @@
+from launch.launch_description import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import UnlessCondition
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    
+    # Launch arguments
+    launch_args = []
+
+    launch_args.append(
+        DeclareLaunchArgument(
+            name="controller_configurations_package",
+            default_value="iisy_config",
+            description="Package that contains controller configurations."
+        )
+    )
+
+    launch_args.append(
+        DeclareLaunchArgument(
+            name="controller_configurations",
+            default_value="config/iisy_controllers.yml",
+            description="Relative path to controller configurations YAML file."
+        )
+    )
+
+    launch_args.append(
+        DeclareLaunchArgument(
+            name="robot_description",
+            description="Robot description XML file."
+        )
+    )
+
+    launch_args.append(
+        DeclareLaunchArgument(
+            name="controller",
+            default_value="position_trajectory_controller",
+            description="Robot controller."
+        )
+    )
+
+    launch_args.append(
+        DeclareLaunchArgument(
+            name="sim",
+            default_value="true",
+            description="Launch robot in simulation or on real setup."
+        )
+    )
+
+    # Configure robot_description
+    robot_description = {"robot_description": LaunchConfiguration("robot_description")}
+
+    # Load controllers from YAML configuration file
+    controller_configurations = PathJoinSubstitution([
+        FindPackageShare(LaunchConfiguration("controller_configurations_package")),
+        LaunchConfiguration("controller_configurations")
+    ])
+
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="screen",
+        parameters=[robot_description]
+    )
+
+    joint_state_broadcaster = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
+    )
+
+    controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[LaunchConfiguration("controller"), "--controller-manager", "/controller_manager"],
+    )
+
+    return LaunchDescription(
+        launch_args + [
+            robot_state_publisher,
+            joint_state_broadcaster,
+            controller
+    ])

src/iisy_config/launch/lbr_move_group.launch.py

@@ -0,0 +1,181 @@
+import os
+import yaml
+
+import traceback
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from ament_index_python import get_package_share_directory
+
+
+def load_yaml(package_name: str, file_path: str):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, "r") as f:
+            return yaml.safe_load(f)
+    except EnvironmentError as error:
+        print("Error loading yaml "+package_name+" | "+file_path)
+        traceback.print_exc()
+        return None
+
+
+def load_file(package_name: str, file_path: str) -> str:
+    package_path = get_package_share_directory(package_name)
+    absolut_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolut_file_path, "r") as f:
+            return f.read()
+    except EnvironmentError:
+        print("Error loading file "+package_name+" | "+file_path)
+        traceback.print_exc()
+        return None
+
+
+def launch_setup(context, *args, **kwargs):
+
+    # Configure robot_description
+    robot_description = {"robot_description": LaunchConfiguration("robot_description")}
+
+    # Robot semantics SRDF
+    robot_description_semantic = {
+        "robot_description_semantic": load_file("iisy_config", "srdf/iisy.srdf")
+    }
+
+    # Kinematics
+    kinematics_yaml = load_yaml("iisy_config", "config/kinematics.yml")
+    
+    # Update group name
+    #kinematics_yaml["{}_arm".format(model)] = kinematics_yaml["group_name"]
+    #del kinematics_yaml["group_name"]
+
+    # Joint limits
+    robot_description_planning = {
+        "robot_description_planning": PathJoinSubstitution(
+            [
+                FindPackageShare("iisy_config"),
+                "config/joint_limits.yml"
+            ]
+        )
+    }
+
+    # Planning
+    ompl_yaml = load_yaml("iisy_config", "config/ompl_planning.yml")
+
+    planning = {
+        "move_group": {
+            "planning_plugin": "ompl_interface/OMPLPlanner",
+            "request_adapters": "default_planner_request_adapters/AddTimeParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints",
+            "start_state_max_bounds_error": 0.1
+        }
+    }
+
+    # Trajectory execution
+    trajectory_execution = {"allow_trajectory_execution": True,
+                            "moveit_manage_controllers": True}
+
+    # Controllers
+    controllers_yaml = load_yaml(
+        LaunchConfiguration("moveit_controller_configurations_package").perform(context),
+        LaunchConfiguration("moveit_controller_configurations").perform(context)
+    )
+
+    moveit_controllers = {"moveit_simple_controller_manager": controllers_yaml,
+                          "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager"}
+
+    # Planning scene
+    planning_scene_monitor_parameters = {"publish_planning_scene": True,
+                                         "publish_geometry_updates": True,
+                                         "publish_state_updates": True,
+                                         "publish_transforms_updates": True}
+
+    # Time configuration
+    use_sim_time = {"use_sim_time": LaunchConfiguration("sim")}
+
+    # Prepare move group node
+    move_group_node = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        arguments=["--ros-args"],
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            kinematics_yaml,
+            robot_description_planning,
+            ompl_yaml,
+            planning,
+            trajectory_execution,
+            moveit_controllers,
+            planning_scene_monitor_parameters,
+            use_sim_time
+        ]
+    )
+
+    # RViz
+    rviz_config = PathJoinSubstitution([FindPackageShare("iisy_config"), "config/config.rviz"])
+
+    rviz = Node(
+        package="rviz2",
+        executable="rviz2",
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            kinematics_yaml,
+            planning,
+            use_sim_time
+        ],
+        arguments=[
+            '-d', rviz_config
+        ]
+    )
+
+    return [
+        move_group_node,
+        rviz
+    ]
+
+def generate_launch_description():
+
+    # Launch arguments
+    launch_args = []
+
+    launch_args.append(
+        DeclareLaunchArgument(
+            name="robot_description",
+            description="Robot description XML file."
+        )
+    )
+
+    launch_args.append(
+        DeclareLaunchArgument(
+            name="moveit_controller_configurations_package",
+            default_value="iisy_config",
+            description="Package that contains MoveIt! controller configurations."
+        )
+    )
+
+    launch_args.append(
+        DeclareLaunchArgument(
+            name="moveit_controller_configurations",
+            default_value="config/iisy_moveit_controllers.yml",
+            description="Relative path to MoveIt! controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name."
+        )
+    )
+
+    launch_args.append(DeclareLaunchArgument(
+        name="sim",
+        default_value="true",
+        description="Launch robot in simulation or on real setup."
+    ))
+
+    return LaunchDescription(
+        launch_args + [
+        OpaqueFunction(function=launch_setup)
+    ])

src/iisy_config/launch/lbr_simulation.launch.py

@@ -0,0 +1,49 @@
+from launch.launch_description import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+
+    # Launch arguments
+    launch_args = []
+
+    launch_args.append(DeclareLaunchArgument(
+        name="robot_name",
+        default_value="iisy",
+        description="Set robot name."
+    ))
+
+    # Launch Gazebo
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare("gazebo_ros"), 
+                "launch", 
+                "gazebo.launch.py"
+            ])
+        )
+    )
+
+    # Note: Environment variable GAZEBO_MODEL_PATH is extended as in 
+    # ROS2 control demos via environment hook https://github.com/ros-controls/ros2_control_demos/tree/master/ros2_control_demo_description/rrbot_description
+    # Also see https://colcon.readthedocs.io/en/released/developer/environment.html#dsv-files
+    # Gazebo launch scripts append GAZEBO_MODEL_PATH with known paths, see https://github.com/ros-simulation/gazebo_ros_pkgs/blob/ab1ae5c05eda62674b36df74eb3be8c93cdc8761/gazebo_ros/launch/gzclient.launch.py#L26
+    spawn_entity = Node(
+        package="gazebo_ros",
+        executable="spawn_entity.py",
+        arguments=[
+            "-topic", "robot_description",
+            "-entity", LaunchConfiguration("robot_name")
+        ],
+        output="screen"
+    )
+
+    return LaunchDescription(
+        launch_args + [
+            gazebo,
+            spawn_entity
+    ])

src/iisy_config/launch/move_group.launch

@@ -1,101 +0,0 @@
-<launch>
-
-  <!-- GDB Debug Option -->
-  <arg name="debug" default="false" />
-  <arg unless="$(arg debug)" name="launch_prefix" value="" />
-  <arg     if="$(arg debug)" name="launch_prefix"
-           value="gdb -x $(dirname)/gdb_settings.gdb --ex run --args" />
-
-  <!-- Verbose Mode Option -->
-  <arg name="info" default="$(arg debug)" />
-  <arg unless="$(arg info)" name="command_args" value="" />
-  <arg     if="$(arg info)" name="command_args" value="--debug" />
-
-  <!-- move_group settings -->
-  <arg name="pipeline" default="ompl" />
-  <arg name="allow_trajectory_execution" default="true"/>
-  <arg name="moveit_controller_manager" default="simple" />
-  <arg name="fake_execution_type" default="interpolate"/>
-  <arg name="max_safe_path_cost" default="1"/>
-  <arg name="publish_monitored_planning_scene" default="true"/>
-
-  <arg name="capabilities" default=""/>
-  <arg name="disable_capabilities" default=""/>
-  <!-- load these non-default MoveGroup capabilities (space seperated) -->
-  <!--
-  <arg name="capabilities" value="
-                a_package/AwsomeMotionPlanningCapability
-                another_package/GraspPlanningPipeline
-                " />
-  -->
-
-  <!-- inhibit these default MoveGroup capabilities (space seperated) -->
-  <!--
-  <arg name="disable_capabilities" value="
-                move_group/MoveGroupKinematicsService
-                move_group/ClearOctomapService
-                " />
-  -->
-
-  <arg name="load_robot_description" default="true" />
-  <!-- load URDF, SRDF and joint_limits configuration -->
-  <include file="$(dirname)/planning_context.launch">
-    <arg name="load_robot_description" value="$(arg load_robot_description)" />
-  </include>
-
-  <!-- Planning Pipelines -->
-  <group ns="move_group/planning_pipelines">
-
-    <!-- OMPL -->
-    <include file="$(dirname)/planning_pipeline.launch.xml">
-      <arg name="pipeline" value="ompl" />
-    </include>
-
-    <!-- CHOMP -->
-    <include file="$(dirname)/planning_pipeline.launch.xml">
-      <arg name="pipeline" value="chomp" />
-    </include>
-
-    <!-- Pilz Industrial Motion -->
-    <include file="$(dirname)/planning_pipeline.launch.xml">
-      <arg name="pipeline" value="pilz_industrial_motion_planner" />
-    </include>
-
-    <!-- Support custom planning pipeline -->
-    <include if="$(eval arg('pipeline') not in ['ompl', 'chomp', 'pilz_industrial_motion_planner'])"
-             file="$(dirname)/planning_pipeline.launch.xml">
-      <arg name="pipeline" value="$(arg pipeline)" />
-    </include>
-  </group>
-
-  <!-- Trajectory Execution Functionality -->
-  <include ns="move_group" file="$(dirname)/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
-    <arg name="moveit_manage_controllers" value="true" />
-    <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
-    <arg name="fake_execution_type" value="$(arg fake_execution_type)" />
-  </include>
-
-  <!-- Sensors Functionality -->
-  <include ns="move_group" file="$(dirname)/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
-    <arg name="moveit_sensor_manager" value="iisy" />
-  </include>
-
-  <!-- Start the actual move_group node/action server -->
-  <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
-    <!-- Set the display variable, in case OpenGL code is used internally -->
-    <env name="DISPLAY" value="$(optenv DISPLAY :0)" />
-
-    <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
-    <param name="sense_for_plan/max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
-    <param name="default_planning_pipeline" value="$(arg pipeline)" />
-    <param name="capabilities" value="$(arg capabilities)" />
-    <param name="disable_capabilities" value="$(arg disable_capabilities)" />
-
-    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
-    <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
-    <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
-    <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
-    <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
-  </node>
-
-</launch>

src/iisy_config/launch/moveit_rviz.launch

@@ -1,15 +0,0 @@
-<launch>
-
-  <arg name="debug" default="false" />
-  <arg unless="$(arg debug)" name="launch_prefix" value="" />
-  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
-
-  <arg name="rviz_config" default="" />
-  <arg     if="$(eval rviz_config=='')" name="command_args" value="" />
-  <arg unless="$(eval rviz_config=='')" name="command_args" value="-d $(arg rviz_config)" />
-
-  <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
-        args="$(arg command_args)" output="screen">
-  </node>
-
-</launch>

src/iisy_config/launch/ompl-chomp_planning_pipeline.launch.xml

@@ -1,19 +0,0 @@
-<launch>
-  <!-- load OMPL planning pipeline, but add the CHOMP planning adapter. -->
-  <include file="$(find panda_moveit_config)/launch/ompl_planning_pipeline.launch.xml">
-    <arg name="planning_adapters" value="
-       default_planner_request_adapters/AddTimeParameterization
-       default_planner_request_adapters/FixWorkspaceBounds
-       default_planner_request_adapters/FixStartStateBounds
-       default_planner_request_adapters/FixStartStateCollision
-       default_planner_request_adapters/FixStartStatePathConstraints
-       chomp/OptimizerAdapter"
-       />
-  </include>
-
-  <!-- load chomp config -->
-  <rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml"/>
-
-  <!-- override trajectory_initialization_method: Use OMPL-generated trajectory -->
-  <param name="trajectory_initialization_method" value="fillTrajectory"/>
-</launch>

src/iisy_config/launch/ompl_planning_pipeline.launch.xml

@@ -1,26 +0,0 @@
-<launch>
-
-  <!-- The request adapters (plugins) used when planning with OMPL. ORDER MATTERS! -->
-  <arg name="planning_adapters"
-       default="default_planner_request_adapters/AddTimeParameterization
-                default_planner_request_adapters/ResolveConstraintFrames
-                default_planner_request_adapters/FixWorkspaceBounds
-                default_planner_request_adapters/FixStartStateBounds
-                default_planner_request_adapters/FixStartStateCollision
-                default_planner_request_adapters/FixStartStatePathConstraints"
-                />
-
-  <arg name="start_state_max_bounds_error" default="0.1" />
-  <arg name="jiggle_fraction" default="0.05" />
-
-  <param name="planning_plugin" value="ompl_interface/OMPLPlanner" />
-  <param name="request_adapters" value="$(arg planning_adapters)" />
-  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
-  <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
-
-  <!-- Add MoveGroup capabilities specific to this pipeline -->
-  <!-- <param name="capabilities" value="" /> -->
-
-  <rosparam command="load" file="$(find iisy_config)/config/ompl_planning.yaml"/>
-
-</launch>

src/iisy_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml

@@ -1,15 +0,0 @@
-<launch>
-
-  <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
-  <arg name="planning_adapters" default="" />
-
-  <param name="planning_plugin" value="pilz_industrial_motion_planner::CommandPlanner" />
-  <param name="request_adapters" value="$(arg planning_adapters)" />
-
-  <!-- Define default planner (for all groups) -->
-  <param name="default_planner_config" value="PTP" />
-
-  <!-- MoveGroup capabilities to load for this pipeline, append sequence capability -->
-  <param name="capabilities" value="pilz_industrial_motion_planner/MoveGroupSequenceAction
-                                    pilz_industrial_motion_planner/MoveGroupSequenceService" />
-</launch>

src/iisy_config/launch/planning_context.launch

@@ -1,26 +0,0 @@
-<launch>
-  <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
-  <arg name="load_robot_description" default="false"/>
-
-  <!-- The name of the parameter under which the URDF is loaded -->
-  <arg name="robot_description" default="robot_description"/>
-
-  <!-- Load universal robot description format (URDF) -->
-  <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find gaz_simulation)/urdf/iisy.urdf"/>
-
-  <!-- The semantic description that corresponds to the URDF -->
-  <param name="$(arg robot_description)_semantic" textfile="$(find iisy_config)/config/iisy.srdf" />
-
-  <!-- Load updated joint limits (override information from URDF) -->
-  <group ns="$(arg robot_description)_planning">
-    <rosparam command="load" file="$(find iisy_config)/config/joint_limits.yaml"/>
-    <rosparam command="load" file="$(find iisy_config)/config/cartesian_limits.yaml"/>
-  </group>
-
-  <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
-  <group ns="$(arg robot_description)_kinematics">
-    <rosparam command="load" file="$(find iisy_config)/config/kinematics.yaml"/>
-
-  </group>
-
-</launch>

src/iisy_config/launch/planning_pipeline.launch.xml

@@ -1,10 +0,0 @@
-<launch>
-
-  <!-- This file makes it easy to include different planning pipelines;
-       It is assumed that all planning pipelines are named XXX_planning_pipeline.launch  -->
-
-  <arg name="pipeline" default="ompl" />
-
-  <include ns="$(arg pipeline)" file="$(dirname)/$(arg pipeline)_planning_pipeline.launch.xml" />
-
-</launch>

src/iisy_config/launch/ros_control_moveit_controller_manager.launch.xml

@@ -1,4 +0,0 @@
-<launch>
-	<!-- Define MoveIt controller manager plugin -->
-	<param name="moveit_controller_manager" value="moveit_ros_control_interface::MoveItControllerManager" />
-</launch>

src/iisy_config/launch/ros_controllers.launch

@@ -1,11 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-
-  <!-- Load joint controller configurations from YAML file to parameter server -->
-  <rosparam file="$(find iisy_config)/config/ros_controllers.yaml" command="load"/>
-
-  <!-- Load the controllers -->
-  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
-    output="screen" args="iisy_controller "/>
-
-</launch>

src/iisy_config/launch/run_benchmark_ompl.launch

@@ -1,21 +0,0 @@
-<launch>
-
-  <!-- This argument must specify the list of .cfg files to process for benchmarking -->
-  <arg name="cfg" />
-
-  <!-- Load URDF -->
-  <include file="$(dirname)/planning_context.launch">
-    <arg name="load_robot_description" value="true"/>
-  </include>
-
-  <!-- Start the database -->
-  <include file="$(dirname)/warehouse.launch">
-    <arg name="moveit_warehouse_database_path" value="moveit_ompl_benchmark_warehouse"/>
-  </include>
-
-  <!-- Start Benchmark Executable -->
-  <node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
-    <rosparam command="load" file="$(find iisy_config)/config/ompl_planning.yaml"/>
-  </node>
-
-</launch>

src/iisy_config/launch/sensor_manager.launch.xml

@@ -1,17 +0,0 @@
-<launch>
-
-  <!-- This file makes it easy to include the settings for sensor managers -->
-
-  <!-- Params for 3D sensors config -->
-  <rosparam command="load" file="$(find iisy_config)/config/sensors_3d.yaml" />
-
-  <!-- Params for the octomap monitor -->
-  <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
-  <param name="octomap_resolution" type="double" value="0.025" />
-  <param name="max_range" type="double" value="5.0" />
-
-  <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
-  <arg name="moveit_sensor_manager" default="iisy" />
-  <include file="$(dirname)/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
-
-</launch>

src/iisy_config/launch/setup_assistant.launch

@@ -1,16 +0,0 @@
-<!-- Re-launch the MoveIt Setup Assistant with this configuration package already loaded -->
-<launch>
-
-  <!-- Debug Info -->
-  <arg name="debug" default="false" />
-  <arg unless="$(arg debug)" name="launch_prefix" value="" />
-  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
-
-  <!-- Run -->
-  <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"
-        args="--config_pkg=iisy_config"
-        launch-prefix="$(arg launch_prefix)"
-        required="true"
-        output="screen" />
-
-</launch>

src/iisy_config/launch/simple_moveit_controller_manager.launch.xml

@@ -1,8 +0,0 @@
-<launch>
-  <!-- Define the MoveIt controller manager plugin to use for trajectory execution -->
-  <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
-
-  <!-- Load controller list to the parameter server -->
-  <rosparam file="$(find iisy_config)/config/simple_moveit_controllers.yaml" />
-  <rosparam file="$(find iisy_config)/config/ros_controllers.yaml" />
-</launch>

src/iisy_config/launch/stomp_planning_pipeline.launch.xml

@@ -1,25 +0,0 @@
-<launch>
-  <!-- Stomp Plugin for MoveIt -->
-  <arg name="planning_plugin" value="stomp_moveit/StompPlannerManager" />
-
-  <arg name="start_state_max_bounds_error" value="0.1" />
-  <arg name="jiggle_fraction" value="0.05" />
-  <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
-  <arg name="planning_adapters"
-       default="default_planner_request_adapters/AddTimeParameterization
-                default_planner_request_adapters/FixWorkspaceBounds
-                default_planner_request_adapters/FixStartStateBounds
-                default_planner_request_adapters/FixStartStateCollision
-                default_planner_request_adapters/FixStartStatePathConstraints" />
-
-
-  <param name="planning_plugin" value="$(arg planning_plugin)" />
-  <param name="request_adapters" value="$(arg planning_adapters)" />
-  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
-  <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
-
-  <!-- Add MoveGroup capabilities specific to this pipeline -->
-  <!-- <param name="capabilities" value="" /> -->
-
-  <rosparam command="load" file="$(find iisy_config)/config/stomp_planning.yaml"/>
-</launch>

src/iisy_config/launch/trajectory_execution.launch.xml

@@ -1,23 +0,0 @@
-<launch>
-  <!-- This file summarizes all settings required for trajectory execution  -->
-
-  <!-- Define moveit controller manager plugin: fake, simple, or ros_control -->
-  <arg name="moveit_controller_manager" />
-  <arg name="fake_execution_type" default="interpolate" />
-
-  <!-- Flag indicating whether MoveIt is allowed to load/unload  or switch controllers -->
-  <arg name="moveit_manage_controllers" default="true"/>
-  <param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
-
-  <!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
-  <param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
-  <!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
-  <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
-  <!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
-  <param name="trajectory_execution/allowed_start_tolerance" value="0.01"/> <!-- default 0.01 -->
-
-  <!-- We use pass_all_args=true here to pass fake_execution_type, which is required by fake controllers, but not by real-robot controllers.
-       As real-robot controller_manager.launch files shouldn't be required to define this argument, we use the trick of passing all args. -->
-  <include file="$(dirname)/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" pass_all_args="true" />
-
-</launch>

src/iisy_config/launch/view_robot.launch.py

@@ -0,0 +1,93 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+# for reference see
+# https://github.com/ros-controls/ros2_control_demos/tree/master/ros2_control_demo_description/rrbot_description
+def generate_launch_description():
+
+    # Launch arguments
+    launch_args = []
+
+    launch_args.append(DeclareLaunchArgument(
+        name='description_package',
+        default_value='iisy_config',
+        description='Description package.'
+    ))
+
+    launch_args.append(DeclareLaunchArgument(
+        name='description_file',
+        default_value='urdf/iisy.urdf',
+        description='Path to URDF file, relative to description_package.'
+    ))
+
+    launch_args.append(DeclareLaunchArgument(
+        name='rviz_config',
+        default_value='config/config.rviz',
+        description='Rviz configuration relative to description_package.'
+    ))
+
+    launch_args.append(DeclareLaunchArgument(
+        name='robot_name',
+        default_value='iisy',
+        description='Set robot name.'
+    ))
+
+    launch_args.append(DeclareLaunchArgument(
+        name='origin_xyz',
+        default_value="'0 0 0'",
+        description='Set position origin of robot.'
+    ))
+
+    launch_args.append(DeclareLaunchArgument(
+        name='origin_rpy',
+        default_value="'0 0 0'",
+        description='Set orientation origin of robot.'
+    ))
+
+    # Load robot description
+    robot_description_content = Command(
+        [
+            FindExecutable(name="xacro"), " ",
+            PathJoinSubstitution(
+                [FindPackageShare(LaunchConfiguration('description_package')), LaunchConfiguration('description_file')]
+            ), " ",
+            "origin_xyz:=", LaunchConfiguration('origin_xyz'), " ",
+            "origin_rpy:=", LaunchConfiguration('origin_rpy'), " ",
+            "robot_name:=", LaunchConfiguration('robot_name')
+        ]
+    )
+
+    robot_description = {'robot_description': robot_description_content}
+
+    # Create required nodes
+    joint_state_publisher_node = Node(
+        package="joint_state_publisher_gui",
+        executable="joint_state_publisher_gui",
+    )
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+    )
+    rviz = Node(
+        package='rviz2',
+        executable='rviz2',
+        parameters=[{'config': PathJoinSubstitution(
+            [FindPackageShare(LaunchConfiguration('description_package')), LaunchConfiguration('rviz_config')]
+        )}]
+    )
+
+    return LaunchDescription(
+        launch_args +
+        [
+            joint_state_publisher_node,
+            robot_state_publisher_node,
+            rviz
+        ]
+    )

src/iisy_config/launch/warehouse.launch

@@ -1,15 +0,0 @@
-<launch>
-
-  <!-- The path to the database must be specified -->
-  <arg name="moveit_warehouse_database_path" />
-
-  <!-- Load warehouse parameters -->
-  <include file="$(dirname)/warehouse_settings.launch.xml" />
-
-  <!-- Run the DB server -->
-  <node name="$(anon mongo_wrapper_ros)" cwd="ROS_HOME" type="mongo_wrapper_ros.py" pkg="warehouse_ros_mongo">
-    <param name="overwrite" value="false"/>
-    <param name="database_path" value="$(arg moveit_warehouse_database_path)" />
-  </node>
-
-</launch>

src/iisy_config/launch/warehouse_settings.launch.xml

@@ -1,16 +0,0 @@
-<launch>
-  <!-- Set the parameters for the warehouse and run the mongodb server. -->
-
-  <!-- The default DB port for moveit (not default MongoDB port to avoid potential conflicts) -->
-  <arg name="moveit_warehouse_port" default="33829" />
-
-  <!-- The default DB host for moveit -->
-  <arg name="moveit_warehouse_host" default="localhost" />
-
-  <!-- Set parameters for the warehouse -->
-  <param name="warehouse_port" value="$(arg moveit_warehouse_port)"/>
-  <param name="warehouse_host" value="$(arg moveit_warehouse_host)"/>
-  <param name="warehouse_exec" value="mongod" />
-  <param name="warehouse_plugin" value="warehouse_ros_mongo::MongoDatabaseConnection" />
-
-</launch>

src/iisy_config/urdf/iisy.urdf

@@ -1,16 +1,19 @@
 <?xml version="1.0" ?>
-<robot name="iisy">
+<robot name="iisy" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:property name="joint_damping" value="10.0"/>
+    <xacro:property name="joint_friction" value="0.1"/>
+
     <link name="world"/>
     <link name="base_link"/>
     <link name="base">
         <collision>
             <geometry>
-                <mesh filename="file://$(find gaz_simulation)/stl/base_bottom_coll.stl"/>
+                <mesh filename="file://$(find iisy_config)/stl/base_bottom_coll.stl"/>
             </geometry>
         </collision>
         <visual>
             <geometry>
-                <mesh filename="file://$(find gaz_simulation)/stl/base_bottom_low.stl"/>
+                <mesh filename="file://$(find iisy_config)/stl/base_bottom_low.stl"/>
             </geometry>
         </visual>
 
@@ -30,7 +33,7 @@
         </collision>
         <visual>
             <geometry>
-                <mesh filename="file://$(find gaz_simulation)/stl/base_top_low.stl"/>
+                <mesh filename="file://$(find iisy_config)/stl/base_top_low.stl"/>
             </geometry>
         </visual>
 
@@ -44,12 +47,12 @@
     <link name="link1_full">
         <collision>
             <geometry>
-                <mesh filename="file://$(find gaz_simulation)/stl/link_1_full_coll.stl"/>
+                <mesh filename="file://$(find iisy_config)/stl/link_1_full_coll.stl"/>
             </geometry>
         </collision>
         <visual>
             <geometry>
-                <mesh filename="file://$(find gaz_simulation)/stl/link_1_full_low.stl"/>
+                <mesh filename="file://$(find iisy_config)/stl/link_1_full_low.stl"/>
             </geometry>
         </visual>
 
@@ -69,7 +72,7 @@
         </collision>
         <visual>
             <geometry>
-                <mesh filename="file://$(find gaz_simulation)/stl/link_2_bottom_low.stl"/>
+                <mesh filename="file://$(find iisy_config)/stl/link_2_bottom_low.stl"/>
             </geometry>
         </visual>
 
@@ -83,12 +86,12 @@
     <link name="link2_top">
         <collision>
             <geometry>
-                <mesh filename="file://$(find gaz_simulation)/stl/link_2_top_coll.stl"/>
+                <mesh filename="file://$(find iisy_config)/stl/link_2_top_coll.stl"/>
             </geometry>
         </collision>
         <visual>
             <geometry>
-                <mesh filename="file://$(find gaz_simulation)/stl/link_2_top_low.stl"/>
+                <mesh filename="file://$(find iisy_config)/stl/link_2_top_low.stl"/>
             </geometry>
         </visual>
 
@@ -102,12 +105,12 @@
     <link name="link3_bottom">
         <collision>
             <geometry>
-                <mesh filename="file://$(find gaz_simulation)/stl/link_3_bottom_coll.stl"/>
+                <mesh filename="file://$(find iisy_config)/stl/link_3_bottom_coll.stl"/>
             </geometry>
         </collision>
         <visual>
             <geometry>
-                <mesh filename="file://$(find gaz_simulation)/stl/link_3_bottom_low.stl"/>
+                <mesh filename="file://$(find iisy_config)/stl/link_3_bottom_low.stl"/>
             </geometry>
         </visual>
 
@@ -121,12 +124,12 @@
     <link name="link3_top">
         <collision>
             <geometry>
-                <mesh filename="file://$(find gaz_simulation)/stl/link_3_top_coll.stl"/>
+                <mesh filename="file://$(find iisy_config)/stl/link_3_top_coll.stl"/>
             </geometry>
         </collision>
         <visual>
             <geometry>
-                <mesh filename="file://$(find gaz_simulation)/stl/link_3_top_low.stl"/>
+                <mesh filename="file://$(find iisy_config)/stl/link_3_top_low.stl"/>
             </geometry>
         </visual>
 
@@ -153,6 +156,7 @@
         <child link="base_top"/>
         <axis xyz="0 0 1"/>
         <limit effort="30" velocity="1.7453292519943"/>
+        <dynamics damping="${joint_damping}" friction="${joint_friction}"/>
     </joint>
     <joint type="revolute" name="base_link1_joint">
         <origin xyz="0 -0.080499 0.092997" rpy="0 0 0"/>
@@ -160,6 +164,7 @@
         <child link="link1_full"/>
         <axis xyz="0 1 0"/>
         <limit effort="30" velocity="1.7453292519943" lower="-2.2689280275926" upper="2.4434609527921"/>
+        <dynamics damping="${joint_damping}" friction="${joint_friction}"/>
     </joint>
     <joint type="revolute" name="link1_link2_joint">
         <origin xyz="0 0 0.3" rpy="0 0 0"/>
@@ -167,6 +172,7 @@
         <child link="link2_bottom"/>
         <axis xyz="0 1 0"/>
         <limit effort="30" velocity="1.7453292519943" lower="-2.6179938779915" upper="2.6179938779915"/>
+        <dynamics damping="${joint_damping}" friction="${joint_friction}"/>
     </joint>
     <joint type="continuous" name="link2_rot">
         <origin xyz="0 0.080501 0.120502" rpy="0 0 0"/>
@@ -174,6 +180,7 @@
         <child link="link2_top"/>
         <axis xyz="0 0 1"/>
         <limit effort="30" velocity="1.7453292519943"/>
+        <dynamics damping="${joint_damping}" friction="${joint_friction}"/>
     </joint>
     <joint type="continuous" name="link2_link3_joint">
         <origin xyz="0 0.0565 0.178003" rpy="0 0 0"/>
@@ -181,6 +188,7 @@
         <child link="link3_bottom"/>
         <axis xyz="0 1 0"/>
         <limit effort="30" velocity="1.7453292519943"/>
+        <dynamics damping="${joint_damping}" friction="${joint_friction}"/>
     </joint>
     <joint type="continuous" name="link3_rot">
         <origin xyz="0 -0.055001 0.085501" rpy="0 0 0"/>
@@ -188,9 +196,10 @@
         <child link="link3_top"/>
         <axis xyz="0 0 1"/>
         <limit effort="30" velocity="1.7453292519943"/>
+        <dynamics damping="${joint_damping}" friction="${joint_friction}"/>
     </joint>
 
-    <transmission name="trans_base_rot">
+    <!--<transmission name="trans_base_rot">
         <type>transmission_interface/SimpleTransmission</type>
         <joint name="base_rot">
             <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
@@ -249,10 +258,150 @@
             <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
             <mechanicalReduction>1</mechanicalReduction>
         </actuator>
-    </transmission>
+    </transmission>-->
+
+    <ros2_control name="lbr_hardware_interface" type="system">
+        <!-- define hardware including parameters, also gazebo -->
+        <hardware>
+            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+        </hardware>
+
+        <!-- define lbr specific state interfaces as sensor, see https://github.com/ros-controls/roadmap/blob/master/design_drafts/components_architecture_and_urdf_examples.md -->
+        <sensor name="iisy_state">
+            <!-- see KUKA::FRI::LBRState -->
+            <state_interface name="sample_time"/>
+            <state_interface name="time_stamp_sec"/>
+            <state_interface name="time_stamp_nano_sec"/>
+        </sensor>
+        <!-- define joints and command/state interfaces for each joint -->
+        <joint name="base_rot">
+            <command_interface name="position">
+                <!-- better to use radians as min max first -->
+                <param name="min">-1</param>
+                <param name="max"> 1</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-1</param>
+                <param name="max"> 1</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="effort"/>
+            <state_interface name="external_torque"/>
+        </joint>
+        <joint name="base_link1_joint">
+            <command_interface name="position">
+                <param name="min">-1</param>
+                <param name="max"> 1</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-1</param>
+                <param name="max"> 1</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="effort"/>
+            <state_interface name="external_torque"/>
+        </joint>
+        <joint name="link1_link2_joint">
+            <command_interface name="position">
+                <param name="min">-1</param>
+                <param name="max"> 1</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-1</param>
+                <param name="max"> 1</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="effort"/>
+            <state_interface name="external_torque"/>
+        </joint>
+        <joint name="link2_rot">
+            <command_interface name="position">
+                <param name="min">-1</param>
+                <param name="max"> 1</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-1</param>
+                <param name="max"> 1</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="effort"/>
+            <state_interface name="external_torque"/>
+        </joint>
+        <joint name="link2_link3_joint">
+            <command_interface name="position">
+                <param name="min">-1</param>
+                <param name="max"> 1</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-1</param>
+                <param name="max"> 1</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="effort"/>
+            <state_interface name="external_torque"/>
+        </joint>
+        <joint name="link3_rot">
+            <command_interface name="position">
+                <param name="min">-1</param>
+                <param name="max"> 1</param>
+            </command_interface>
+            <command_interface name="effort">
+                <param name="min">-1</param>
+                <param name="max"> 1</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="effort"/>
+            <state_interface name="external_torque"/>
+        </joint>
+    </ros2_control>
+
     <gazebo>
-        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+        <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
+            <parameters>$(find iisy_config)/config/iisy_controllers.yml</parameters>
             <robotNamespace>/iisy</robotNamespace>
         </plugin>
     </gazebo>
+
+    <gazebo reference="base">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/Orange</material>
+    </gazebo>
+
+    <gazebo reference="base_top">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/Orange</material>
+    </gazebo>
+
+    <gazebo reference="link1_full">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/Orange</material>
+    </gazebo>
+
+    <gazebo reference="link2_bottom">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/Orange</material>
+    </gazebo>
+
+    <gazebo reference="link2_top">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/Orange</material>
+    </gazebo>
+
+    <gazebo reference="link3_bottom">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/Orange</material>
+    </gazebo>
+    
+    <gazebo reference="link3_top">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/Grey</material>
+    </gazebo>
+
 </robot>

src/moveit2_tutorials

@@ -1 +0,0 @@
-Subproject commit e02236a8096f540c356504a5ceeb1a08212421e9

src/moveit_visual_tools

@@ -1 +0,0 @@
-Subproject commit ef16670fab41acb5a9f265ce2de6d7875c6358eb