.gitlab-ci.yml

@@ -26,7 +26,7 @@ build-colcon-job:
   - mv * .src/robossembler-ros2
   - mv .git .src/robossembler-ros2
   - mv .src src
-  - vcs import src < src/robossembler-ros2/rbs.sim.repos
+  - vcs import src < src/robossembler-ros2/repos/sim.rbs.repos
   - rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble
   - colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
   rules:

README.md

@@ -26,20 +26,27 @@ Prepare workspace & install dependencies (So far only tested with UR robot arm)
 ```bash
 mkdir -p ~/robossembler_ws/src && cd ~/robossembler_ws/src
 git clone https://gitlab.com/robosphere/robossembler-ros2
-vcs import . < robossembler-ros2/rbs.sim.repos
+vcs import . < robossembler-ros2/repos/sim.rbs.repos
 cd ..
 rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble
 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install
 ```
-<!-- For simulation with Mujoco, pls [ref](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/blob/fdaa9a2dd0ce15fc949b127dd1164095504273fb/cartesian_controller_simulation/README.md) build and install section. -->
+
+Additionally, if you want to use Cartesian controllers, such as stiffness or others, you need to execute the following:
+```
+# in directory robossembler_ws/src
+vcs import . < robossembler-ros2/repos/cartesian_controllers.repos
+```
+This will also install `ros2_control` and `gz_ros2_control` as packages, so it is recommended to delete global packages if they have been installed.
 
 ### Set Gazebo enviroment variables
 Replace `[WS_FOLDER]` with your workspace folder
 ```bash
-echo "export IGN_GAZEBO_RESOURCE_PATH=${IGN_GAZEBO_RESOURCE_PATH}:~/[WS_FOLDER]/install/robotiq_description/share/:~/[WS_FOLDER]/install/rbs_simulation/share/rbs_simulation/" >> ~/.bashrc
+echo "export IGN_GAZEBO_RESOURCE_PATH=${IGN_GAZEBO_RESOURCE_PATH}:~/[WS_FOLDER]/install/rbs_simulation/share/rbs_simulation/" >> ~/.bashrc
+# or if you have alredy built the workspace
+echo "export IGN_GAZEBO_RESOURCE_PATH=${IGN_GAZEBO_RESOURCE_PATH}:~/$(ros2 pkg prefix rbs_simulation)/share/rbs_simulation/" >> ~/.bashrc
 ```
 
-
 ### Examples
 Activate current ROS2 enviroment:
 ```

env_manager/gz_enviroment/src/gz_enviroment.cpp

@@ -41,7 +41,7 @@ CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
   m_topic_name = std::string("/world/") + m_world_name + "/dynamic_pose/info";
   m_service_spawn = std::string("/world/") + m_world_name + "/create";
   m_config_loader = std::make_shared<rbs_utils::AssemblyConfigLoader>(
-      "asp-example2", getNode());
+      "asp-example", getNode());
   m_follow_frames = m_config_loader->getSceneModelNames();
   // m_target_places = std::make_shared<tf2_msgs::msg::TFMessage>();
   m_transforms = m_config_loader->getTfData("bishop");
@@ -92,8 +92,6 @@ CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State &) {
 
 // TODO: Check to do this via EntityComponentManager
 void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V &pose_v) {
-  // TODO: Read from config
-  // m_follow_frames = {"box1", "box2", "box3", "box4", "box5", "box6"};
   std::vector<geometry_msgs::msg::TransformStamped> all_transforms{};
 
   for (const auto &it : pose_v.pose()) {

rbs_bringup/config/rbs.rviz

@@ -6,9 +6,8 @@ Panels:
       Expanded:
         - /Global Options1
         - /TF1/Frames1
-        - /MarkerArray2
       Splitter Ratio: 0.49496981501579285
-    Tree Height: 981
+    Tree Height: 985
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -76,62 +75,37 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
-          base:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
           base_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          base_link_inertia:
+            Value: true
+          ee_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          flange:
+          fork0_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          forearm_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          ft_frame:
+          fork1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          inner_rgbd_camera:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          outer_rgbd_camera:
+          fork2_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          schunk_grasp_point:
+          main0_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          schunk_gripper_base_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          schunk_l_finger_link:
+          main1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          schunk_r_finger_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          shoulder_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
@@ -140,77 +114,37 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          upper_arm_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
           world:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          wrist_1_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          wrist_2_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          wrist_3_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
         Robot Alpha: 1
         Show Robot Collision: false
-        Show Robot Visual: true
+        Show Robot Visual: false
       Value: true
     - Class: rviz_default_plugins/TF
       Enabled: true
       Frame Timeout: 15
       Frames:
         All Enabled: false
-        base:
-          Value: false
         base_link:
           Value: false
-        base_link_inertia:
+        ee_link:
-          Value: false
-        flange:
-          Value: false
-        forearm_link:
-          Value: false
-        ft_frame:
-          Value: false
-        inner_rgbd_camera:
-          Value: false
-        outer_rgbd_camera:
-          Value: false
-        schunk_grasp_point:
           Value: true
-        schunk_gripper_base_link:
+        fork0_link:
           Value: true
-        schunk_l_finger_link:
+        fork1_link:
           Value: true
-        schunk_r_finger_link:
+        fork2_link:
+          Value: true
+        main0_link:
+          Value: true
+        main1_link:
           Value: true
-        shoulder_link:
-          Value: false
         tool0:
           Value: false
-        upper_arm_link:
-          Value: false
         world:
           Value: false
-        wrist_1_link:
-          Value: false
-        wrist_2_link:
-          Value: false
-        wrist_3_link:
-          Value: false
       Marker Scale: 0.4000000059604645
       Name: TF
       Show Arrows: true
@@ -219,30 +153,14 @@ Visualization Manager:
       Tree:
         world:
           base_link:
-            base:
+            fork0_link:
-              {}
+              main0_link:
-            base_link_inertia:
+                fork1_link:
-              shoulder_link:
+                  main1_link:
-                upper_arm_link:
+                    fork2_link:
-                  forearm_link:
+                      ee_link:
-                    wrist_1_link:
+                        tool0:
-                      wrist_2_link:
+                          {}
-                        wrist_3_link:
-                          flange:
-                            tool0:
-                              inner_rgbd_camera:
-                                {}
-                              schunk_grasp_point:
-                                {}
-                              schunk_gripper_base_link:
-                                schunk_l_finger_link:
-                                  {}
-                                schunk_r_finger_link:
-                                  {}
-                          ft_frame:
-                            {}
-          outer_rgbd_camera:
-            {}
       Update Interval: 0
       Value: true
     - Class: moveit_rviz_plugin/Trajectory
@@ -255,62 +173,37 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        base:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
         base_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        base_link_inertia:
+          Value: true
+        ee_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        flange:
+        fork0_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        forearm_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        ft_frame:
+        fork1_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        inner_rgbd_camera:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        outer_rgbd_camera:
+        fork2_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        schunk_grasp_point:
+        main0_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        schunk_gripper_base_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        schunk_l_finger_link:
+        main1_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        schunk_r_finger_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        shoulder_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
@@ -319,30 +212,10 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        upper_arm_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
         world:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        wrist_1_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        wrist_2_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        wrist_3_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
       Loop Animation: false
       Name: Trajectory
       Robot Alpha: 0.5
@@ -368,6 +241,84 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: workspace
       Value: true
+    - Class: rviz_default_plugins/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Interactive Markers Namespace: /motion_controller_handle
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      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
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        ee_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fork0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fork1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fork2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        main0_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        main1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tool0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -414,7 +365,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 6.619869709014893
+      Distance: 2.528358221054077
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -429,10 +380,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5753980875015259
+      Pitch: 0.5803980827331543
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 1.1453965902328491
+      Yaw: 1.0903966426849365
     Saved: ~
 Window Geometry:
   Displays:
@@ -440,7 +391,7 @@ Window Geometry:
   Height: 1385
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000025f000004cdfc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000004cd0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032c000002170000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000007a00ffffff000000010000015f000004cdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000004cd0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000052d000004cd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Tool Properties:
@@ -450,5 +401,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 2307
-  X: 436
+  X: 507
-  Y: 66
+  Y: 315

rbs_bringup/launch/bringup.launch.py

@@ -1,5 +1,4 @@
-import os
+from launch import LaunchDescription, condition
-from launch import LaunchDescription
 from launch.actions import (
     DeclareLaunchArgument,
     IncludeLaunchDescription,
@@ -9,7 +8,6 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
-# from rbs_launch_utils.launch_common import load_yaml_abs
 
 def generate_launch_description():
     declared_arguments = []
@@ -17,50 +15,22 @@ def generate_launch_description():
         DeclareLaunchArgument(
             "rbs_robot_type",
             description="Type of robot by name",
-            choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
+            choices=["rbs_arm","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
-            default_value="ur5e",
+            default_value="rbs_arm",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "safety_limits",
-            default_value="true",
-            description="Enables the safety limits controller if true.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "safety_pos_margin",
-            default_value="0.15",
-            description="The margin to lower and upper limits in the safety controller.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "safety_k_position",
-            default_value="20",
-            description="k-position factor in the safety controller.",
         )
     )
     # General arguments
     declared_arguments.append(
         DeclareLaunchArgument(
             "controllers_file",
-            default_value="ur_plus_gripper_controllers.yaml",
+            default_value="rbs_arm_controllers_gazebosim.yaml",
             description="YAML file with the controllers configuration.",
         )
     )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "controllers_with_gripper_file",
-            default_value="ur_plus_gripper_controllers.yaml",
-            description="YAML file with the UR + gripper_controller configuration.",
-        )
-    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "description_package",
-            default_value="ur_description",
+            default_value="rbs_arm",
             description="Description package with robot URDF/XACRO files. Usually the argument \
         is not set, it enables use of a custom description.",
         )
@@ -68,7 +38,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "description_file",
-            default_value="ur.urdf.xacro",
+            default_value="rbs_arm_modular.xacro",
             description="URDF/XACRO description file with the robot.",
         )
     )
@@ -95,19 +65,10 @@ def generate_launch_description():
             description="Robot controller to start.",
         )
     )
-    
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "initial_gripper_controller",
-            default_value="gripper_controller",
-            description="Robot controller to start.",
-        )
-    )
-
     declared_arguments.append(
         DeclareLaunchArgument(
             "moveit_config_package",
-            default_value="ur_moveit_config",
+            default_value="rbs_arm",
             description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
         is not set, it enables use of a custom moveit config.",
         )
@@ -115,7 +76,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "moveit_config_file",
-            default_value="ur.srdf.xacro",
+            default_value="rbs_arm.srdf.xacro",
             description="MoveIt SRDF/XACRO description file with the robot.",
         )
     )
@@ -123,48 +84,75 @@ def generate_launch_description():
         DeclareLaunchArgument(
             "use_sim_time",
             default_value="true",
-            description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.",
+            description="Make MoveIt to use simulation time.\
+            This is needed for the trajectory planing in simulation.",
         )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("with_gripper", default_value="true", description="With gripper or not?")
+        DeclareLaunchArgument("with_gripper", 
+                              default_value="true", 
+                              description="With gripper or not?")
     )
     declared_arguments.append(
-        DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
+        DeclareLaunchArgument("launch_rviz", 
+                              default_value="true", 
+                              description="Launch RViz?")
     )
     declared_arguments.append(
-        DeclareLaunchArgument("sim_gazebo", default_value="true", description="Gazebo Simulation")
+        DeclareLaunchArgument("sim_gazebo", 
+                              default_value="true", 
+                              description="Gazebo Simulation")
     )
     declared_arguments.append(
-        DeclareLaunchArgument("sim_mujoco", default_value="false", description="Mujoco Simulation")
+        DeclareLaunchArgument("env_manager", 
+                              default_value="true", 
+                              description="Launch env_manager?")
     )
     declared_arguments.append(
-        DeclareLaunchArgument("sim_fake", default_value="false", description="Mock contollers")
+        DeclareLaunchArgument("launch_sim", 
+                              default_value="true", 
+                              description="Launch simulator (Gazebo)?\
+                              Most general arg")
     )
     declared_arguments.append(
-        DeclareLaunchArgument("env_manager", default_value="true", description="Launch env_manager?")
+        DeclareLaunchArgument("launch_moveit", 
+                              default_value="true", 
+                              description="Launch moveit?")
     )
     declared_arguments.append(
-        DeclareLaunchArgument("launch_sim", default_value="true", description="Launch simulator (Gazebo)? Most general arg")
+        DeclareLaunchArgument("launch_perception", 
+                              default_value="false", 
+                              description="Launch perception?")
     )
     declared_arguments.append(
-        DeclareLaunchArgument("launch_moveit", default_value="true", description="Launch moveit?")
+        DeclareLaunchArgument("launch_task_planner", 
+                              default_value="false", 
+                              description="Launch task_planner?")
     )
     declared_arguments.append(
-        DeclareLaunchArgument("launch_perception", default_value="false", description="Launch perception?")
+        DeclareLaunchArgument("cartesian_controllers", 
+                              default_value="false", 
+                              description="Load cartesian\
+                              controllers?")
     )
     declared_arguments.append(
-        DeclareLaunchArgument("launch_task_planner", default_value="false", description="Launch task_planner?")
+        DeclareLaunchArgument("hardware", 
+                              choices=["gazebo", "mock"],
+                              default_value="gazebo", 
+                              description="Choose your harware_interface")
     )
     declared_arguments.append(
-        DeclareLaunchArgument("cartesian_controllers", default_value="false", description="Load cartesian controllers?")
+        DeclareLaunchArgument("launch_controllers", 
+                              default_value="true", 
+                              description="Launch controllers?")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("gazebo_gui", 
+                              default_value="false", 
+                              description="Launch gazebo with gui?")
     )
-
     # Initialize Arguments
     rbs_robot_type = LaunchConfiguration("rbs_robot_type")
-    safety_limits = LaunchConfiguration("safety_limits")
-    safety_pos_margin = LaunchConfiguration("safety_pos_margin")
-    safety_k_position = LaunchConfiguration("safety_k_position")
     # General arguments
     with_gripper_condition = LaunchConfiguration("with_gripper")
     controllers_file = LaunchConfiguration("controllers_file")
@@ -183,10 +171,10 @@ def generate_launch_description():
     moveit_config_file = LaunchConfiguration("moveit_config_file")
     use_sim_time = LaunchConfiguration("use_sim_time")
     sim_gazebo = LaunchConfiguration("sim_gazebo")
-    sim_mujoco = LaunchConfiguration("sim_mujoco")
+    hardware = LaunchConfiguration("hardware")
-    sim_fake = LaunchConfiguration("sim_fake")
     env_manager = LaunchConfiguration("env_manager")
-
+    launch_controllers = LaunchConfiguration("launch_controllers")
+    gazebo_gui = LaunchConfiguration("gazebo_gui")
 
     initial_joint_controllers_file_path = PathJoinSubstitution(
         [FindPackageShare(description_package), "config", controllers_file]
@@ -195,44 +183,36 @@ def generate_launch_description():
     rviz_config_file = PathJoinSubstitution(
         [FindPackageShare("rbs_bringup"), "config", "rbs.rviz"]
     )
-
     
     robot_description_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
             " ",
-            PathJoinSubstitution(
+            PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
-                [FindPackageShare(description_package), "urdf", description_file]
-            ),
             " ",
-            "safety_limits:=", safety_limits, " ",
+            "gripper_name:=", "", " ",
-            "safety_pos_margin:=", safety_pos_margin, " ",
-            "safety_k_position:=", safety_k_position, " ",
-            "name:=", "ur", " ",
-            "ur_type:=", rbs_robot_type, " ",
             "prefix:=", prefix, " ",
-            "sim_mujoco:=", sim_mujoco, " ",
+            "hardware:=", hardware, " ",
-            "sim_gazebo:=", sim_gazebo, " ",
-            "sim_fake:=", sim_fake, " ",
             "simulation_controllers:=", initial_joint_controllers_file_path, " ",
-            "with_gripper:=", with_gripper_condition, " ",
+
         ]
     )
+
     robot_description = {"robot_description": robot_description_content}
     
-    # MoveIt Configuration
     robot_description_semantic_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
             " ",
             PathJoinSubstitution(
-                [FindPackageShare(moveit_config_package), "srdf", moveit_config_file]
+                [FindPackageShare(moveit_config_package), "config/moveit", "rbs_arm.srdf.xacro"]
-            ), " ",
+            ),
-            "name:=", "ur", " ",
+            " ",
-            "prefix:=", prefix, " ",
+            "name:=","rbs_arm"," ",
-            "with_gripper:=", with_gripper_condition
+            "prefix:=",prefix," ",
         ]
     )
+ 
     robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
     robot_description_kinematics = PathJoinSubstitution(
         [FindPackageShare(moveit_config_package), "config", "kinematics.yaml"]
@@ -260,7 +240,7 @@ def generate_launch_description():
     control = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([
             PathJoinSubstitution([
-                FindPackageShare('ur_description'),
+                FindPackageShare('rbs_arm'),
                 'launch',
                 'control.launch.py'
             ])
@@ -272,7 +252,8 @@ def generate_launch_description():
             'initial_joint_controller': initial_joint_controller,
             'controllers_file': controllers_file,
             "cartesian_controllers": cartesian_controllers
-        }.items())
+        }.items(),
+        condition=IfCondition(launch_controllers))
     
     simulation = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([
@@ -284,15 +265,17 @@ def generate_launch_description():
         ]),
         launch_arguments={
             'sim_gazebo': sim_gazebo,
+            'gazebo_gui': gazebo_gui,
             'rbs_robot_type': rbs_robot_type,
-            'env_manager': env_manager
+            'env_manager': env_manager,
+            'debugger': "false"
         }.items(),
         condition=IfCondition(launch_simulation))
     
     moveit = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([
             PathJoinSubstitution([
-                FindPackageShare('ur_moveit_config'),
+                FindPackageShare('rbs_arm'),
                 'launch',
                 'moveit.launch.py'
             ])
@@ -322,7 +305,6 @@ def generate_launch_description():
             'robot_description_kinematics': robot_description_kinematics,
             'use_sim_time': use_sim_time,
             'with_gripper_condition': with_gripper_condition,
-            'points_params_filepath': "gripperPositions.yaml"
         }.items()
     )
     
@@ -363,7 +345,3 @@ def generate_launch_description():
         perception
     ]
     return LaunchDescription(declared_arguments + nodes_to_start)
-
-    
-    
-    

rbs_bt_executor/bt_trees/example_skills/env_manager_starter.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root main_tree_to_execute="Main">
+  <Action ID="EnvStarter"
+    env_class="gz_enviroment::GzEnviroment"
+    env_name="gz_enviroment"
+    server_name="/env_manager/start_env"
+    server_timeout="1000"
+    workspace="{workspace}" />
+
+  <TreeNodesModel>
+    <Action ID="EnvStarter">
+      <input_port name="env_class" />
+      <input_port name="env_name" />
+      <input_port name="server_name" />
+      <input_port name="server_timeout" />
+      <output_port name="workspace" />
+    </Action>
+  </TreeNodesModel>
+</root>

rbs_bt_executor/bt_trees/example_skills/move_to_joint_states.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root main_tree_to_execute="Main">
+  <Action ID="MoveToPose"
+    robot_name="{robot_name}"
+    JointState="{joint_state}"
+    server_name="{action_server}"
+    server_timeout="{time_s}"
+    cancel_timeout="{time_c}" />
+
+  <TreeNodesModel>
+    <Action ID="MoveToPose">
+      <!-- Массив положений суставов робота-->
+      <!-- std::vector<double>-->
+      <input_port name="JointState" />
+      <!-- std::string название робота. Нужен для MoveIt2 -->
+      <input_port name="robot_name" />
+      <!-- Для ros2_action-->
+      <input_port name="cancel_timeout" />
+      <input_port name="server_name" />
+      <input_port name="server_timeout" />
+    </Action>
+  </TreeNodesModel>
+</root>

rbs_bt_executor/bt_trees/example_skills/move_to_pose.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root main_tree_to_execute="Main">
+  <Action ID="MoveToPose"
+    robot_name="{robot_name}"
+    pose="{pose}"
+    server_name="{action_server}"
+    server_timeout="{time_s}"
+    cancel_timeout="{time_c}" />
+
+  <TreeNodesModel>
+    <Action ID="MoveToPose">
+      <!-- Непосредственно сама позиция в формате ROS2 сообщения-->
+      <!-- geometry_msgs::msg::Pose-->
+      <input_port name="pose" />
+      <!-- std::string название робота. Нужен для MoveIt2 -->
+      <input_port name="robot_name" />
+      <!-- Для ros2_action-->
+      <input_port name="cancel_timeout" />
+      <input_port name="server_name" />
+      <input_port name="server_timeout" />
+    </Action>
+  </TreeNodesModel>
+</root>

rbs_bt_executor/bt_trees/example_skills/move_to_pose_array.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root main_tree_to_execute="Main">
+  <Action ID="MoveToPoseArray"
+    cancel_timeout="5000" 
+    pose_vec_in="{goal_pose}"
+    pose_vec_out="{goal_pose}"
+    robot_name="{robot_name}" 
+    server_name="{server_name}" 
+    server_timeout="5000" />
+  <TreeNodesModel>
+    <Action ID="MoveToPose">
+      <!-- Непосредственно сама позиция в формате ROS2 сообщения-->
+      <!-- geometry_msgs::msgs::PoseArray-->
+      <input_port name="pose_vec_in" />
+      <!-- Выдает вычтенную первую позицию из массива-->
+      <!-- geometry_msgs::msgs::PoseArray-->
+      <output_port name="pose_vec_out" />
+      <!-- std::string название робота. Нужен для MoveIt2 -->
+      <input_port name="robot_name" />
+      <!-- Для ros2_action-->
+      <input_port name="cancel_timeout" />
+      <input_port name="server_name" />
+      <input_port name="server_timeout" />
+    </Action>
+  </TreeNodesModel>
+</root>

rbs_bt_executor/bt_trees/nodes_interfaces/general.xml

@@ -0,0 +1,51 @@
+<root>
+  <TreeNodesModel>
+    <Action ID="EnvStarter">
+      <input_port name="env_class" />
+      <input_port name="env_name" />
+      <input_port name="server_name" />
+      <input_port name="server_timeout" />
+      <output_port name="workspace" />
+    </Action>
+
+    <Action ID="MoveToPose">
+      <!-- Массив положений суставов робота-->
+      <!-- std::vector<double>-->
+      <input_port name="JointState" />
+      <!-- std::string название робота. Нужен для MoveIt2 -->
+      <input_port name="robot_name" />
+      <!-- Для ros2_action-->
+      <input_port name="cancel_timeout" />
+      <input_port name="server_name" />
+      <input_port name="server_timeout" />
+    </Action>
+
+    <Action ID="MoveToPose">
+      <!-- Непосредственно сама позиция в формате ROS2 сообщения-->
+      <!-- geometry_msgs::msg::Pose-->
+      <input_port name="pose" />
+      <!-- std::string название робота. Нужен для MoveIt2 -->
+      <input_port name="robot_name" />
+      <!-- Для ros2_action-->
+      <input_port name="cancel_timeout" />
+      <input_port name="server_name" />
+      <input_port name="server_timeout" />
+    </Action>
+
+    <Action ID="MoveToPoseArray">
+      <!-- Непосредственно сама позиция в формате ROS2 сообщения-->
+      <!-- geometry_msgs::msgs::PoseArray-->
+      <input_port name="pose_vec_in" />
+      <!-- Выдает вычтенную первую позицию из массива-->
+      <!-- geometry_msgs::msgs::PoseArray-->
+      <output_port name="pose_vec_out" />
+      <!-- std::string название робота. Нужен для MoveIt2 -->
+      <input_port name="robot_name" />
+      <!-- Для ros2_action-->
+      <input_port name="cancel_timeout" />
+      <input_port name="server_name" />
+      <input_port name="server_timeout" />
+    </Action>
+    
+  </TreeNodesModel>
+</root>

rbs_bt_executor/bt_trees/test_tree.xml

@@ -6,7 +6,7 @@
             <Action ID="EnvStarter" env_class="gz_enviroment::GzEnviroment" env_name="gz_enviroment"
                 server_name="/env_manager/start_env" server_timeout="1000" workspace="{workspace}" />
             <SubTreePlus ID="WorkspaceInspection" __autoremap="1" goal_pose="{workspace}"
-                robot_name="ur_manipulator" />
+                robot_name="rbs_arm" />
         </Sequence>
     </BehaviorTree>
     <!-- ////////// -->
@@ -24,33 +24,13 @@
             <Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
                 pose_vec_out="{goal_pose}"
                 robot_name="{robot_name}" server_name="/move_cartesian" server_timeout="5000" />
+            <Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
+                pose_vec_out="{goal_pose}"
+                robot_name="{robot_name}" server_name="/move_cartesian" server_timeout="5000" />
+
         </Sequence>
     </BehaviorTree>
     <!-- ////////// -->
-    <TreeNodesModel>
+  <include path="./nodes_interfaces/general.xml"/>
-        <Action ID="EnvStarter">
-            <inout_port name="env_class" />
-            <inout_port name="env_name" />
-            <inout_port name="server_name" />
-            <inout_port name="server_timeout" />
-            <inout_port name="workspace" />
-        </Action>
-        <Action ID="MoveToPose">
-            <inout_port name="cancel_timeout" />
-            <inout_port name="pose" />
-            <inout_port name="robot_name" />
-            <inout_port name="server_name" />
-            <inout_port name="server_timeout" />
-        </Action>
-        <Action ID="MoveToPoseArray">
-            <inout_port name="cancel_timeout" />
-            <inout_port name="pose_vec_in" />
-            <inout_port name="robot_name" />
-            <inout_port name="server_name" />
-            <inout_port name="server_timeout" />
-        </Action>
-        <SubTree ID="PoseEstimation" />
-        <SubTree ID="WorkspaceInspection" />
-    </TreeNodesModel>
     <!-- ////////// -->
 </root>

rbs_bt_executor/src/MoveToPoseArray.cpp

@@ -30,7 +30,8 @@ public:
       goal.target_pose = target_pose_vec_.poses.at(0);
 
       target_pose_vec_.poses.erase(target_pose_vec_.poses.begin());
-      setOutput<geometry_msgs::msg::PoseArray>("pose_vec_out", target_pose_vec_);
+      setOutput<geometry_msgs::msg::PoseArray>("pose_vec_out",
+                                               target_pose_vec_);
     } else {
       RCLCPP_WARN(_node->get_logger(), "Target pose vector empty");
     }
@@ -39,11 +40,10 @@ public:
   }
 
   static BT::PortsList providedPorts() {
-    return providedBasicPorts({
+    return providedBasicPorts(
-        BT::InputPort<std::string>("robot_name"),
+        {BT::InputPort<std::string>("robot_name"),
-        BT::InputPort<geometry_msgs::msg::PoseArray>("pose_vec_in"),
+         BT::InputPort<geometry_msgs::msg::PoseArray>("pose_vec_in"),
-        BT::OutputPort<geometry_msgs::msg::PoseArray>("pose_vec_out")
+         BT::OutputPort<geometry_msgs::msg::PoseArray>("pose_vec_out")});
-    });
   }
 
 private:

rbs_simulation/launch/simulation.launch.py

@@ -12,27 +12,38 @@ from ament_index_python.packages import get_package_share_directory
 def generate_launch_description():
     declared_arguments = []
     declared_arguments.append(
-        DeclareLaunchArgument("sim_gazebo", default_value="false", description="Gazebo Simulation")
+        DeclareLaunchArgument("sim_gazebo",
+                              default_value="false",
+                              description="Gazebo Simulation")
     )
     declared_arguments.append(
-        DeclareLaunchArgument(
+         DeclareLaunchArgument("rbs_robot_type",
-            "rbs_robot_type",
+                              description="Type of robot by name",
-            description="Type of robot by name",
+                              choices=["rbs_arm" ,"ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
-            choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
+                              default_value="rbs_arm",)
-            default_value="ur5e",
-        )
     )
     declared_arguments.append(
-        DeclareLaunchArgument("env_manager", default_value="false", description="Launch env_manager?")
+        DeclareLaunchArgument("env_manager",
+                              default_value="false",
+                              description="Launch env_manager?")
     )
     declared_arguments.append(
-        DeclareLaunchArgument("gazebo_gui", default_value="false", description="Launch env_manager?")
+        DeclareLaunchArgument("gazebo_gui",
+                              default_value="true",
+                              description="Launch env_manager?")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("debugger",
+                              default_value="false",
+                              description="launch Gazebo with debugger?")
     )
     
     sim_gazebo = LaunchConfiguration("sim_gazebo")
     rbs_robot_type = LaunchConfiguration("rbs_robot_type")
     env_manager_cond = LaunchConfiguration("env_manager")
     gazebo_gui = LaunchConfiguration("gazebo_gui")
+    debugger = LaunchConfiguration("debugger")
+
     # FIXME: To args when we'll have different files
     # TODO: Use global variable to find world file in robossembler_db
     world_config_file = PathJoinSubstitution(
@@ -44,31 +55,38 @@ def generate_launch_description():
             PythonLaunchDescriptionSource(
                 [os.path.join(get_package_share_directory('ros_gz_sim'),
                             'launch', 'gz_sim.launch.py')]),
-            launch_arguments=[('ign_args', [' -r ',world_config_file, " -s"])],
+            launch_arguments={
+                'gz_args': [' -r ',world_config_file, " -s"],
+                "debugger": debugger,
+            }.items(),
             condition=UnlessCondition(gazebo_gui))
     
     gazebo = IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
                 [os.path.join(get_package_share_directory('ros_gz_sim'),
                             'launch', 'gz_sim.launch.py')]),
-            launch_arguments=[('ign_args', [' -r ',world_config_file])],
+            launch_arguments={
+                'gz_args': [' -r ',world_config_file],
+                "debugger": debugger,
+            }.items(),
             condition=IfCondition(gazebo_gui))
     
     # Spawn robot
-    gazebo_spawn_robot = Node(package='ros_ign_gazebo', executable='create',
+    gazebo_spawn_robot = Node(
-                 arguments=[
+        package='ros_gz_sim',
-                    '-name', rbs_robot_type,
+        executable='create',
-                    '-x', '0.0',
+        arguments=[
-                    '-z', '0.0',
+            '-name', rbs_robot_type,
-                    '-y', '0.0',
+            '-x', '0.0',
-                    '-topic', '/robot_description'],
+            '-z', '0.0',
-                 output='screen',
+            '-y', '0.0',
-                 condition=IfCondition(sim_gazebo))
+            '-topic', '/robot_description'],
+        output='screen',
+        condition=IfCondition(sim_gazebo))
     env_manager = Node(package="env_manager", 
                        executable="run_env_manager",
-                       condition=IfCondition(env_manager_cond)
+                       condition=IfCondition(env_manager_cond))
-                       )
+
-    
     # Bridge
     rgbd_bridge_out = Node(
         package='ros_gz_bridge',
@@ -80,8 +98,7 @@ def generate_launch_description():
             '/outer_rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked'
         ],
         output='screen',
-        condition=IfCondition(sim_gazebo)
+        condition=IfCondition(sim_gazebo))
-    )
     
     rgbd_bridge_in = Node(
         package='ros_gz_bridge',
@@ -93,9 +110,14 @@ def generate_launch_description():
             '/inner_rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked'
         ],
         output='screen',
-    )
+        condition=IfCondition(sim_gazebo))
-    
     
+    clock_bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'],
+        output='screen',
+        condition=IfCondition(sim_gazebo))
 
     nodes_to_start = [
         gazebo,
@@ -103,6 +125,7 @@ def generate_launch_description():
         gazebo_spawn_robot,
         env_manager,
         rgbd_bridge_out,
-        rgbd_bridge_in
+        rgbd_bridge_in,
+        clock_bridge,
     ]
     return LaunchDescription(declared_arguments + nodes_to_start)

rbs_simulation/mujoco_model/current_mj.xml

@@ -2,6 +2,9 @@
   <compiler angle="radian" meshdir="meshes/"/>
   <default/>
   <asset>
+    <texture type="skybox" builtin="gradient" rgb1="0.9 0.9 0.9" rgb2="0.5 0.5 0.5" width="512" height="512"/>
+    <texture name="texplane" type="2d" builtin="checker" rgb1=".25 .25 .25" rgb2=".3 .3 .3" width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
+    <material name="matplane" reflectance="0.2" texture="texplane" texrepeat="1 1" texuniform="true"/>
     <mesh name="base" file="base.stl"/>
     <mesh name="shoulder" file="shoulder.stl"/>
     <mesh name="upperarm" file="upperarm.stl"/>
@@ -11,6 +14,8 @@
     <mesh name="wrist3" file="wrist3.stl"/>
   </asset>
   <worldbody>
+    <light directional="true" diffuse=".8 .8 .8" specular=".2 .2 .2" pos="0 0 5" dir="0 0 -1" castshadow="false"/>
+    <geom name="floor" pos="0 0 -0.0" size="0 0 1" type="plane" material="matplane"/>
     <geom quat="-1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" mesh="base"/>
     <geom quat="-1 0 0 0" type="mesh" mesh="base"/>
     <body name="shoulder_link" pos="0 0 0.1625" quat="0 0 0 1" >

rbs_simulation/worlds/mir.sdf

@@ -12,7 +12,8 @@
     <plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
     <plugin name="ignition::gazebo::systems::Sensors" filename="ignition-gazebo-sensors-system">
         <render_engine>ogre2</render_engine>
-      </plugin>  
+      </plugin>
+    <plugin name='ignition::gazebo::systems::ForceTorque' filename='ignition-gazebo-forcetorque-system'/>
     <gravity>0 0 -9.8</gravity>
     <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
     <atmosphere type='adiabatic'/>

rbs_utils/src/rbs_utils.cpp

@@ -349,10 +349,10 @@ AssemblyConfigLoader::getWorkspaceInspectorTrajectory() {
     if (json.contains("workspace")) {
       auto workspace = readWorkspaceJson(json);
       for (auto &point : workspace.transforms) {
-        geometry_msgs::msg::Pose pose;
+        auto pose = transformTrajectory(point);
-        pose = transformTrajectory(point);
         pose_array.poses.push_back(pose);
       }
+      pose_array.poses.push_back(transformTrajectory(workspace.transforms[0]));
     }
   }
   return pose_array;
@@ -361,9 +361,9 @@ AssemblyConfigLoader::getWorkspaceInspectorTrajectory() {
 geometry_msgs::msg::Pose AssemblyConfigLoader::transformTrajectory(
     const geometry_msgs::msg::TransformStamped &pose) {
   auto pose_eigen = tf2::transformToEigen(pose.transform);
-  Eigen::AngleAxisd rotation(M_PI, Eigen::Vector3d::UnitX());
+  Eigen::AngleAxisd rotation(M_PI, Eigen::Vector3d::UnitY());
   pose_eigen.rotate(rotation);
-  pose_eigen.translation().z() += 0.5;
+  pose_eigen.translation().z() += 0.35;
   auto pose_msg = tf2::toMsg(pose_eigen);
   return pose_msg;
 }

repos/cartesian_controllers.repos

@@ -0,0 +1,13 @@
+repositories:
+  cartesian-controllers:
+    type: git
+    url: https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers.git
+    version: ros2
+  ros2_control:
+    type: git
+    url: https://github.com/solid-sinusoid/gz_ros2_control.git
+    version: gz-ros2-cartesian-controllers
+  gz_ros2_control:
+    type: git
+    url: https://github.com/solid-sinusoid/gz_ros2_control.git
+    version: pass-robot-description

repos/real.repos

@@ -15,7 +15,3 @@ repositories:
     type: git
     url: https://gitlab.com/robossembler/arm-tools/urdf-model-shrunk-gripper-egp-40-n-n-b.git
     version: 2-add-ros2-control
-  gz_ros2_control:
-    type: git
-    url: https://github.com/solid-sinusoid/gz_ros2_control.git
-    version: ft-sensor-broadcaster

repos/sim.rbs.repos

@@ -0,0 +1,13 @@
+repositories:
+  rbs_arm:
+    type: git
+    url: https://github.com/solid-sinusoid/rbs-arm.git
+    version: main
+  rbs_gripper:
+    type: git
+    url: https://github.com/solid-sinusoid/rbs-gripper.git
+    version: main
+  behavior_tree:
+    type: git
+    url: https://github.com/solid-sinusoid/behavior_tree.git
+    version: master

repos/sim.ur.repos

@@ -15,7 +15,3 @@ repositories:
     type: git
     url: https://gitlab.com/robossembler/arm-tools/urdf-model-shrunk-gripper-egp-40-n-n-b.git
     version: 2-add-ros2-control
-  gz_ros2_control:
-    type: git
-    url: https://github.com/solid-sinusoid/gz_ros2_control.git
-    version: ft-sensor-broadcaster