Working arm

src/arm_harverster/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.8)
+project(arm_harverster)
+
+find_package(ament_cmake REQUIRED)
+
+# Installs your Python script file and marks it executable
+install(PROGRAMS
+  scripts/move_to_point.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# Installs your launch file directory
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

src/arm_harverster/LICENSE

@@ -0,0 +1,25 @@
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+
+   * Neither the name of the copyright holder nor the names of its
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.

src/arm_harverster/config/moveit_py.yaml

@@ -0,0 +1,30 @@
+moveit_py:
+  ros__parameters:
+    # Planning scene monitor configurations
+    planning_scene_monitor_options:
+      name: "planning_scene_monitor"
+      robot_description: "robot_description"
+      joint_states_topic: "/joint_states"
+
+    # MoveItCpp configurations
+    moveit_cpp_options:
+      planning_scene_monitor_options:
+        name: "planning_scene_monitor"
+        robot_description: "robot_description"
+        joint_states_topic: "/joint_states"
+      planning_pipelines:
+        - ompl
+
+    # Define the planning pipeline properties
+    ompl:
+      planning_plugins:
+        - ompl_interface/OMPLPlanner
+      request_adapters:
+        - default_planning_request_adapters/AddTimeOptimalParameterization
+        - default_planning_request_adapters/ResolveConstraintFrames
+        - default_planning_request_adapters/FixWorkspaceBounds
+        - default_planning_request_adapters/FixStartStateBounds
+        - default_planning_request_adapters/FixStartStateCollision
+        - default_planning_request_adapters/FixStartStatePathConstraints
+      response_adapters:
+        - default_planning_request_adapters/DisplayMotionPath

src/arm_harverster/launch/run_move_to_point.launch.py

@@ -0,0 +1,13 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package="arm_harverster",
+            executable="move_to_point.py",
+            name="arm_point_controller",
+            output="screen",
+            parameters=[{"use_sim_time": True}]
+        )
+    ])

src/arm_harverster/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>arm_harverster</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="marcin.matczak@student.put.poznan.pl">marcin</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/arm_harverster/scripts/move_to_point.py

@@ -0,0 +1,85 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionClient
+from moveit_msgs.action import MoveGroup
+from moveit_msgs.msg import Constraints, PositionConstraint, OrientationConstraint, BoundingVolume
+from shape_msgs.msg import SolidPrimitive
+from geometry_msgs.msg import PoseStamped
+
+class ArmController(Node):
+    def __init__(self):
+        super().__init__('arm_point_controller')
+        
+        # Create an action client for the MoveGroup action server
+        self._action_client = ActionClient(self, MoveGroup, '/move_action')
+        self.get_logger().info("Waiting for move_group action server...")
+        self._action_client.wait_for_server()
+        self.get_logger().info("Connected to move_group server!")
+
+    def send_goal(self):
+        goal_msg = MoveGroup.Goal()
+        
+        # 1. Configure general planning workspace/group settings
+        goal_msg.request.group_name = "arm"
+        goal_msg.request.allowed_planning_time = 5.0
+        goal_msg.request.num_planning_attempts = 5
+        goal_msg.request.pipeline_id = "ompl"
+        
+        # 2. Define our target pose
+        target_pose = PoseStamped()
+        target_pose.header.frame_id = "world"
+        target_pose.pose.position.x = 0.2
+        target_pose.pose.position.y = 0.1
+        target_pose.pose.position.z = 0.4
+        target_pose.pose.orientation.w = 1.0  # Facing neutral forward
+
+        # 3. Package the target pose into MoveIt's expected constraints format
+        position_constraint = PositionConstraint()
+        position_constraint.header.frame_id = "world"
+        position_constraint.link_name = "tool_link" # Make sure this matches your tip link name!
+        
+        # Define a small target volume region around the point (0.01m tolerance)
+        box = SolidPrimitive()
+        box.type = SolidPrimitive.BOX
+        box.dimensions = [0.01, 0.01, 0.01]
+        
+        volume = BoundingVolume()
+        volume.primitives.append(box)
+        volume.primitive_poses.append(target_pose.pose)
+        position_constraint.constraint_region = volume
+        position_constraint.weight = 1.0
+
+        # Bundle constraints together
+        goal_constraints = Constraints()
+        goal_constraints.position_constraints.append(position_constraint)
+        goal_msg.request.goal_constraints.append(goal_constraints)
+
+        # 4. Fire the goal request over to MoveIt
+        self.get_logger().info("Sending goal coordinates to MoveIt...")
+        self._send_goal_future = self._action_client.send_goal_async(goal_msg)
+        self._send_goal_future.add_done_callback(self.goal_response_callback)
+
+    def goal_response_callback(self, future):
+        goal_handle = future.result()
+        if not goal_handle.accepted:
+            self.get_logger().error("Goal rejected by MoveIt planning pipeline!")
+            return
+
+        self.get_logger().info("Goal accepted! Planning and executing path...")
+        self._get_result_future = goal_handle.get_result_async()
+        self._get_result_future.add_done_callback(self.get_result_callback)
+
+    def get_result_callback(self, future):
+        result = future.result().result
+        self.get_logger().info("Trajectory Execution Finished!")
+        rclpy.shutdown()
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ArmController()
+    node.send_goal()
+    rclpy.spin(node)
+
+if __name__ == '__main__':
+    main()

src/arm_moveit_config/.setup_assistant

@@ -0,0 +1,25 @@
+moveit_setup_assistant_config:
+  urdf:
+    package: robotarm_description
+    relative_path: urdf/robot_arm.urdf.xacro
+  srdf:
+    relative_path: config/six_axis_arm.srdf
+  package_settings:
+    author_name: marcin
+    author_email: 113255@example.com
+    generated_timestamp: 1780594876
+  control_xacro:
+    command:
+      - position
+    state:
+      - position
+      - velocity
+  modified_urdf:
+    xacros:
+      - control_xacro
+  control_xacro:
+    command:
+      - position
+    state:
+      - position
+      - velocity

src/arm_moveit_config/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.22)
+project(arm_moveit_config)
+
+find_package(ament_cmake REQUIRED)
+
+ament_package()
+
+if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch")
+  install(
+    DIRECTORY launch
+    DESTINATION share/${PROJECT_NAME}
+    PATTERN "setup_assistant.launch" EXCLUDE)
+endif()
+
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
+install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME})

src/arm_moveit_config/config/initial_positions.yaml

@@ -0,0 +1,9 @@
+# Default initial positions for six_axis_arm's ros2_control fake system
+
+initial_positions:
+  joint_1: 0
+  joint_2: 0
+  joint_3: 0
+  joint_4: 0
+  joint_5: 0
+  joint_6: 0

src/arm_moveit_config/config/joint_limits.yaml

@@ -0,0 +1,40 @@
+# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+
+# For beginners, we downscale velocity and acceleration limits.
+# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
+default_velocity_scaling_factor: 0.1
+default_acceleration_scaling_factor: 0.1
+
+# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+joint_limits:
+  joint_1:
+    has_velocity_limits: true
+    max_velocity: 2.5
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+  joint_2:
+    has_velocity_limits: true
+    max_velocity: 2.0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+  joint_3:
+    has_velocity_limits: true
+    max_velocity: 2.5
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+  joint_4:
+    has_velocity_limits: true
+    max_velocity: 3.0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+  joint_5:
+    has_velocity_limits: true
+    max_velocity: 3.0
+    has_acceleration_limits: true
+    max_acceleration: 5.0
+  joint_6:
+    has_velocity_limits: true
+    max_velocity: 4.0
+    has_acceleration_limits: true
+    max_acceleration: 5.0

src/arm_moveit_config/config/kinematics.yaml

@@ -0,0 +1,4 @@
+arm:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.0050000000000000001
+  kinematics_solver_timeout: 0.0050000000000000001

src/arm_moveit_config/config/moveit.rviz

@@ -0,0 +1,51 @@
+Panels:
+  - Class: rviz_common/Displays
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /MotionPlanning1
+  - Class: rviz_common/Help
+    Name: Help
+  - Class: rviz_common/Views
+    Name: Views
+Visualization Manager:
+  Displays:
+    - Class: rviz_default_plugins/Grid
+      Name: Grid
+      Value: true
+    - Class: moveit_rviz_plugin/MotionPlanning
+      Name: MotionPlanning
+      Planned Path:
+        Loop Animation: true
+        State Display Time: 0.05 s
+        Trajectory Topic: display_planned_path
+      Planning Scene Topic: monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 1
+      Scene Robot:
+        Robot Alpha: 0.5
+      Value: true
+  Global Options:
+    Fixed Frame: world
+  Tools:
+    - Class: rviz_default_plugins/Interact
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 2.0
+      Focal Point:
+        X: -0.1
+        Y: 0.25
+        Z: 0.30
+      Name: Current View
+      Pitch: 0.5
+      Target Frame: world
+      Yaw: -0.623
+Window Geometry:
+  Height: 975
+  QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Width: 1200

src/arm_moveit_config/config/moveit_controllers.yaml

@@ -0,0 +1,19 @@
+# MoveIt uses this configuration for controller management
+
+moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+
+moveit_simple_controller_manager:
+  controller_names:
+    - arm_controller
+
+  arm_controller:
+    type: FollowJointTrajectory
+    action_ns: follow_joint_trajectory
+    default: True
+    joints:
+      - joint_1
+      - joint_2
+      - joint_3
+      - joint_4
+      - joint_5
+      - joint_6

src/arm_moveit_config/config/pilz_cartesian_limits.yaml

@@ -0,0 +1,6 @@
+# Limits for the Pilz planner
+cartesian_limits:
+  max_trans_vel: 1.0
+  max_trans_acc: 2.25
+  max_trans_dec: -5.0
+  max_rot_vel: 1.57

src/arm_moveit_config/config/ros2_controllers.yaml

@@ -0,0 +1,27 @@
+# This config file is used by ros2_control
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+arm_controller:
+  ros__parameters:
+    joints:
+      - joint_1
+      - joint_2
+      - joint_3
+      - joint_4
+      - joint_5
+      - joint_6
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    allow_nonzero_velocity_at_trajectory_end: true

src/arm_moveit_config/config/six_axis_arm.ros2_control.xacro

@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="six_axis_arm_ros2_control" params="name initial_positions_file">
+        <xacro:property name="initial_positions" value="${xacro.load_yaml(initial_positions_file)['initial_positions']}"/>
+
+        <ros2_control name="${name}" type="system">
+            <hardware>
+                <!-- By default, set up controllers for simulation. This won't work on real hardware -->
+                <plugin>mock_components/GenericSystem</plugin>
+            </hardware>
+            <joint name="joint_1">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['joint_1']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="joint_2">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['joint_2']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="joint_3">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['joint_3']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="joint_4">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['joint_4']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="joint_5">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['joint_5']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="joint_6">
+                <command_interface name="position"/>
+                <state_interface name="position">
+                  <param name="initial_value">${initial_positions['joint_6']}</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+
+        </ros2_control>
+    </xacro:macro>
+</robot>

src/arm_moveit_config/config/six_axis_arm.srdf

@@ -0,0 +1,33 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot name="six_axis_arm">
+    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+    <group name="arm">
+        <chain base_link="base_link" tip_link="tool_link"/>
+    </group>
+    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
+    <end_effector name="tip" parent_link="tool_link" group="arm"/>
+    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
+    <virtual_joint name="world" type="fixed" parent_frame="world" child_link="base_link"/>
+    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+    <disable_collisions link1="base_link" link2="link_1" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="link_2" reason="Never"/>
+    <disable_collisions link1="link_1" link2="link_2" reason="Adjacent"/>
+    <disable_collisions link1="link_2" link2="link_3" reason="Adjacent"/>
+    <disable_collisions link1="link_2" link2="link_4" reason="Never"/>
+    <disable_collisions link1="link_2" link2="link_5" reason="Never"/>
+    <disable_collisions link1="link_2" link2="link_6" reason="Never"/>
+    <disable_collisions link1="link_3" link2="link_4" reason="Adjacent"/>
+    <disable_collisions link1="link_3" link2="link_5" reason="Never"/>
+    <disable_collisions link1="link_3" link2="link_6" reason="Never"/>
+    <disable_collisions link1="link_4" link2="link_5" reason="Adjacent"/>
+    <disable_collisions link1="link_4" link2="link_6" reason="Never"/>
+    <disable_collisions link1="link_5" link2="link_6" reason="Adjacent"/>
+</robot>

src/arm_moveit_config/config/six_axis_arm.urdf.xacro

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="six_axis_arm">
+    <xacro:arg name="initial_positions_file" default="initial_positions.yaml" />
+
+    <!-- Import six_axis_arm urdf file -->
+    <xacro:include filename="$(find robotarm_description)/urdf/robot_arm.urdf.xacro" />
+
+    <!-- Import control_xacro -->
+    <xacro:include filename="six_axis_arm.ros2_control.xacro" />
+
+
+    <xacro:six_axis_arm_ros2_control name="FakeSystem" initial_positions_file="$(arg initial_positions_file)"/>
+
+</robot>

src/arm_moveit_config/launch/demo.launch.py

@@ -0,0 +1,7 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_demo_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder("six_axis_arm", package_name="arm_moveit_config").to_moveit_configs()
+    return generate_demo_launch(moveit_config)

src/arm_moveit_config/launch/move_group.launch.py

@@ -0,0 +1,8 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_move_group_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder("six_axis_arm", package_name="arm_moveit_config").to_moveit_configs()
+    moveit_config.robot_description_kinematics["use_sim_time"] = True
+    return generate_move_group_launch(moveit_config)

src/arm_moveit_config/launch/moveit_rviz.launch.py

@@ -0,0 +1,7 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_moveit_rviz_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder("six_axis_arm", package_name="arm_moveit_config").to_moveit_configs()
+    return generate_moveit_rviz_launch(moveit_config)

src/arm_moveit_config/launch/rsp.launch.py

@@ -0,0 +1,7 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_rsp_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder("six_axis_arm", package_name="arm_moveit_config").to_moveit_configs()
+    return generate_rsp_launch(moveit_config)

src/arm_moveit_config/launch/setup_assistant.launch.py

@@ -0,0 +1,7 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_setup_assistant_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder("six_axis_arm", package_name="arm_moveit_config").to_moveit_configs()
+    return generate_setup_assistant_launch(moveit_config)

src/arm_moveit_config/launch/sim.launch.py

@@ -0,0 +1,119 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch.actions import IncludeLaunchDescription, TimerAction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+    # 1. Define package directories (Adjust names if yours differ)
+    moveit_config_pkg = get_package_share_directory('arm_moveit_config')
+    
+    # 2. Include the standard MoveIt 2 move_group launch file
+    # This automatically loads the robot description (URDF/XACRO) and handles robot_state_publisher
+    move_group_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(moveit_config_pkg, 'launch', 'move_group.launch.py')
+        )
+    )
+
+    # 3. Launch Gazebo Sim (Modern Gazebo) empty world
+    gazebo_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')
+        ),
+        launch_arguments={'gz_args': '-r empty.sdf'}.items(), # '-r' runs simulation immediately
+    )
+
+    # 4. Spawn the robot entity inside the Gazebo world
+    # It listens to the /robot_description topic published by MoveIt's robot_state_publisher
+    spawn_robot = Node(
+        package='ros_gz_sim',
+        executable='create',
+        arguments=[
+            '-topic', 'robot_description',
+            '-name', 'six_axis_arm',
+            '-z', '0.0' # Adjust if your base needs to sit higher
+        ],
+        output='screen',
+    )
+
+    # 5. Define ros2_control Spawners
+    # Broadcaster for joint states (vital for MoveIt to know where the arm currently is)
+    joint_state_broadcaster_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
+    )
+
+    # Trajectory controller for sending trajectory paths to Gazebo
+    arm_controller_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['arm_controller', '--controller-manager', '/controller_manager'],
+    )
+
+    # 6. Bridge Gazebo Clock to ROS 2 (Crucial for simulation time syncing)
+    bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
+        output='screen'
+    )
+
+    # 7. Delay controller spawning until AFTER the robot is spawned in Gazebo
+    # This prevents ros2_control from crashing due to Gazebo not being ready yet
+    delay_joint_state_broadcaster = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=spawn_robot,
+            on_exit=[joint_state_broadcaster_spawner],
+        )
+    )
+
+    delay_arm_controller = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[arm_controller_spawner],
+        )
+    )
+    delay_move_group_launch = TimerAction(
+        period=5.0,
+        actions=[move_group_launch]
+    )
+
+    import xacro
+
+    # ... inside generate_launch_description() ...
+
+    # 1. Path to your actual Xacro file
+    xacro_file = os.path.join(
+        get_package_share_directory('robotarm_description'), # Change to your description pkg
+        'urdf',
+        'robot_arm.urdf.xacro' # Change to your actual file name
+    )
+
+    # 2. Process the xacro file into raw XML text
+    robot_description_raw = xacro.process_file(xacro_file).toxml()
+
+    # 3. Add the explicit Robot State Publisher Node
+    robot_state_publisher_node = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[{'robot_description': robot_description_raw, 'use_sim_time': True}]
+    )
+
+
+    return LaunchDescription([
+        gazebo_launch,
+        delay_move_group_launch,
+        #move_group_launch,
+        spawn_robot,
+        bridge,
+        delay_joint_state_broadcaster,
+        delay_arm_controller,
+        robot_state_publisher_node,
+    ])

src/arm_moveit_config/launch/spawn_controllers.launch.py

@@ -0,0 +1,7 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_spawn_controllers_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder("six_axis_arm", package_name="arm_moveit_config").to_moveit_configs()
+    return generate_spawn_controllers_launch(moveit_config)

src/arm_moveit_config/launch/static_virtual_joint_tfs.launch.py

@@ -0,0 +1,7 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder("six_axis_arm", package_name="arm_moveit_config").to_moveit_configs()
+    return generate_static_virtual_joint_tfs_launch(moveit_config)

src/arm_moveit_config/launch/warehouse_db.launch.py

@@ -0,0 +1,7 @@
+from moveit_configs_utils import MoveItConfigsBuilder
+from moveit_configs_utils.launches import generate_warehouse_db_launch
+
+
+def generate_launch_description():
+    moveit_config = MoveItConfigsBuilder("six_axis_arm", package_name="arm_moveit_config").to_moveit_configs()
+    return generate_warehouse_db_launch(moveit_config)

src/arm_moveit_config/package.xml

@@ -0,0 +1,52 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>arm_moveit_config</name>
+  <version>0.3.0</version>
+  <description>
+     An automatically generated package with all the configuration and launch files for using the six_axis_arm with the MoveIt Motion Planning Framework
+  </description>
+  <maintainer email="113255@example.com">marcin</maintainer>
+
+  <license>BSD-3-Clause</license>
+
+  <url type="website">http://moveit.ros.org/</url>
+  <url type="bugtracker">https://github.com/moveit/moveit2/issues</url>
+  <url type="repository">https://github.com/moveit/moveit2</url>
+
+  <author email="113255@example.com">marcin</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>moveit_ros_move_group</exec_depend>
+  <exec_depend>moveit_kinematics</exec_depend>
+  <exec_depend>moveit_planners</exec_depend>
+  <exec_depend>moveit_simple_controller_manager</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>xacro</exec_depend>
+  <!-- The next 2 packages are required for the gazebo simulation.
+       We don't include them by default to prevent installing gazebo and all its dependencies. -->
+  <!-- <exec_depend>joint_trajectory_controller</exec_depend> -->
+  <!-- <exec_depend>gazebo_ros_control</exec_depend> -->
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>moveit_configs_utils</exec_depend>
+  <exec_depend>moveit_ros_move_group</exec_depend>
+  <exec_depend>moveit_ros_visualization</exec_depend>
+  <exec_depend>moveit_ros_warehouse</exec_depend>
+  <exec_depend>moveit_setup_assistant</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>robotarm_description</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>rviz_common</exec_depend>
+  <exec_depend>rviz_default_plugins</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>warehouse_ros_mongo</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+
+  <export>
+      <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/robotarm_description/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.8)
+project(robotarm_description)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+install(
+  DIRECTORY urdf config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/robotarm_description/LICENSE

@@ -0,0 +1,25 @@
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+
+   * Neither the name of the copyright holder nor the names of its
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.

src/robotarm_description/config/ros2_controllers.yaml

@@ -0,0 +1,26 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100 # Hz
+
+    arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+arm_controller:
+  ros__parameters:
+    joints:
+      - joint_1
+      - joint_2
+      - joint_3
+      - joint_4
+      - joint_5
+      - joint_6
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    open_loop_control: true
+    allow_integration_in_errors: true

src/robotarm_description/config/sensors_3d.yaml

@@ -0,0 +1,2 @@
+sensors:
+  - {}

src/robotarm_description/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>robotarm_description</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="marcin.matczak@student.put.poznan.pl">marcin</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/robotarm_description/urdf/robot_arm.urdf.xacro

@@ -0,0 +1,255 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="six_axis_arm">
+
+  <xacro:property name="PI" value="3.1415926535897931"/>
+  
+  <material name="grey"><color rgba="0.2 0.2 0.2 1.0"/></material>
+  <material name="orange"><color rgba="1.0 0.42 0.04 1.0"/></material>
+  <material name="blue"><color rgba="0.05 0.35 0.65 1.0"/></material>
+
+  <xacro:macro name="cylinder_inertial" params="mass radius length *origin">
+    <inertial>
+      <xacro:insert_block name="origin"/>
+      <mass value="${mass}"/>
+      <inertia 
+        ixx="${(1/12) * mass * (3*radius*radius + length*length)}" ixy="0.0" ixz="0.0"
+        iyy="${(1/12) * mass * (3*radius*radius + length*length)}" iyz="0.0"
+        izz="${(1/2) * mass * radius * radius}"/>
+    </inertial>
+  </xacro:macro>
+
+  <link name="world"/>
+
+  <joint name="world_to_base" type="fixed">
+    <parent link="world"/>
+    <child link="base_link"/>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+  </joint>
+
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0.05" rpy="0 0 0"/>
+      <geometry><cylinder radius="0.15" length="0.1"/></geometry>
+      <material name="grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.05" rpy="0 0 0"/>
+      <geometry><cylinder radius="0.15" length="0.1"/></geometry>
+    </collision>
+    <xacro:cylinder_inertial mass="5.0" radius="0.15" length="0.1">
+      <origin xyz="0 0 0.05" rpy="0 0 0"/>
+    </xacro:cylinder_inertial>
+  </link>
+
+  <joint name="joint_1" type="revolute">
+    <parent link="base_link"/>
+    <child link="link_1"/>
+    <origin xyz="0 0 0.1" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-${PI}" upper="${PI}" effort="300.0" velocity="2.5"/>
+    <dynamics damping="1.0" friction="1.0"/>
+  </joint>
+
+  <link name="link_1">
+    <visual>
+      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <geometry><cylinder radius="0.08" length="0.3"/></geometry>
+      <material name="orange"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <geometry><cylinder radius="0.08" length="0.3"/></geometry>
+    </collision>
+    <xacro:cylinder_inertial mass="4.0" radius="0.08" length="0.3">
+      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+    </xacro:cylinder_inertial>
+  </link>
+
+  <joint name="joint_2" type="revolute">
+    <parent link="link_1"/>
+    <child link="link_2"/>
+    <origin xyz="0 0 0.3" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.0" upper="2.0" effort="300.0" velocity="2.0"/>
+    <dynamics damping="1.0" friction="1.0"/>
+  </joint>
+
+  <link name="link_2">
+    <visual>
+      <origin xyz="0 0 0.2" rpy="0 0 0"/>
+      <geometry><cylinder radius="0.07" length="0.4"/></geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.2" rpy="0 0 0"/>
+      <geometry><cylinder radius="0.07" length="0.4"/></geometry>
+    </collision>
+    <xacro:cylinder_inertial mass="3.5" radius="0.07" length="0.4">
+      <origin xyz="0 0 0.2" rpy="0 0 0"/>
+    </xacro:cylinder_inertial>
+  </link>
+
+  <joint name="joint_3" type="revolute">
+    <parent link="link_2"/>
+    <child link="link_3"/>
+    <origin xyz="0 0 0.4" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.5" upper="2.5" effort="200.0" velocity="2.5"/>
+    <dynamics damping="1.0" friction="1.0"/>
+  </joint>
+
+  <link name="link_3">
+    <visual>
+      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <geometry><cylinder radius="0.06" length="0.3"/></geometry>
+      <material name="orange"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <geometry><cylinder radius="0.06" length="0.3"/></geometry>
+    </collision>
+    <xacro:cylinder_inertial mass="2.5" radius="0.06" length="0.3">
+      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+    </xacro:cylinder_inertial>
+  </link>
+
+  <joint name="joint_4" type="revolute">
+    <parent link="link_3"/>
+    <child link="link_4"/>
+    <origin xyz="0 0 0.3" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-${PI}" upper="${PI}" effort="100.0" velocity="3.0"/>
+    <dynamics damping="0.5" friction="0.5"/>
+  </joint>
+
+  <link name="link_4">
+    <visual>
+      <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <geometry><cylinder radius="0.05" length="0.2"/></geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <geometry><cylinder radius="0.05" length="0.2"/></geometry>
+    </collision>
+    <xacro:cylinder_inertial mass="1.8" radius="0.05" length="0.2">
+      <origin xyz="0 0 0.1" rpy="0 0 0"/>
+    </xacro:cylinder_inertial>
+  </link>
+
+  <joint name="joint_5" type="revolute">
+    <parent link="link_4"/>
+    <child link="link_5"/>
+    <origin xyz="0 0 0.2" rpy="0 0 0"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-2.0" upper="2.0" effort="100.0" velocity="3.0"/>
+    <dynamics damping="0.5" friction="0.5"/>
+  </joint>
+
+  <link name="link_5">
+    <visual>
+      <origin xyz="0 0 0.05" rpy="0 0 0"/>
+      <geometry><cylinder radius="0.04" length="0.1"/></geometry>
+      <material name="grey"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.05" rpy="0 0 0"/>
+      <geometry><cylinder radius="0.04" length="0.1"/></geometry>
+    </collision>
+    <xacro:cylinder_inertial mass="1.0" radius="0.04" length="0.1">
+      <origin xyz="0 0 0.05" rpy="0 0 0"/>
+    </xacro:cylinder_inertial>
+  </link>
+
+  <joint name="joint_6" type="revolute">
+    <parent link="link_5"/>
+    <child link="link_6"/>
+    <origin xyz="0 0 0.1" rpy="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-${PI}" upper="${PI}" effort="50.0" velocity="4.0"/>
+    <dynamics damping="0.2" friction="0.2"/>
+  </joint>
+
+  <link name="link_6">
+    <visual>
+      <origin xyz="0 0 0.02" rpy="0 0 0"/>
+      <geometry><cylinder radius="0.03" length="0.04"/></geometry>
+      <material name="orange"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.02" rpy="0 0 0"/>
+      <geometry><cylinder radius="0.03" length="0.04"/></geometry>
+    </collision>
+    <xacro:cylinder_inertial mass="0.5" radius="0.03" length="0.04">
+      <origin xyz="0 0 0.02" rpy="0 0 0"/>
+    </xacro:cylinder_inertial>
+  </link>
+
+  <link name="tool_link"/>
+  <joint name="tool_joint" type="fixed">
+    <parent link="link_6"/>
+    <child link="tool_link"/>
+    <origin xyz="0 0 0.04" rpy="0 0 0"/>
+  </joint>
+
+  <ros2_control name="GazeboSimSystem" type="system">
+    <hardware>
+      <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+    </hardware>
+    
+    <joint name="joint_1">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+    </joint>
+
+    <joint name="joint_2">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+    </joint>
+
+    <joint name="joint_3">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+    </joint>
+
+    <joint name="joint_4">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+    </joint>
+
+    <joint name="joint_5">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+    </joint>
+
+    <joint name="joint_6">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+    </joint>
+  </ros2_control>
+
+  <gazebo>
+    <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+      <parameters>$(find robotarm_description)/config/ros2_controllers.yaml</parameters>
+    </plugin>
+  </gazebo>
+
+</robot>