commit 5b46c410319ab493d30480eed1f80188c3b90b4a Author: Marcin M Date: Thu Jun 4 21:48:44 2026 +0200 Working arm diff --git a/src/arm_harverster/CMakeLists.txt b/src/arm_harverster/CMakeLists.txt new file mode 100644 index 0000000..b1bb59e --- /dev/null +++ b/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() \ No newline at end of file diff --git a/src/arm_harverster/LICENSE b/src/arm_harverster/LICENSE new file mode 100644 index 0000000..574ef07 --- /dev/null +++ b/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. diff --git a/src/arm_harverster/config/moveit_py.yaml b/src/arm_harverster/config/moveit_py.yaml new file mode 100644 index 0000000..5504c37 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/arm_harverster/launch/run_move_to_point.launch.py b/src/arm_harverster/launch/run_move_to_point.launch.py new file mode 100755 index 0000000..dcc0063 --- /dev/null +++ b/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}] + ) + ]) \ No newline at end of file diff --git a/src/arm_harverster/package.xml b/src/arm_harverster/package.xml new file mode 100644 index 0000000..46ede07 --- /dev/null +++ b/src/arm_harverster/package.xml @@ -0,0 +1,18 @@ + + + + arm_harverster + 0.0.0 + TODO: Package description + marcin + BSD-3-Clause + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/arm_harverster/scripts/move_to_point.py b/src/arm_harverster/scripts/move_to_point.py new file mode 100755 index 0000000..f64d551 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/src/arm_moveit_config/.setup_assistant b/src/arm_moveit_config/.setup_assistant new file mode 100644 index 0000000..eff8475 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/arm_moveit_config/CMakeLists.txt b/src/arm_moveit_config/CMakeLists.txt new file mode 100644 index 0000000..73836ca --- /dev/null +++ b/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}) diff --git a/src/arm_moveit_config/config/initial_positions.yaml b/src/arm_moveit_config/config/initial_positions.yaml new file mode 100644 index 0000000..79ffd9b --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/arm_moveit_config/config/joint_limits.yaml b/src/arm_moveit_config/config/joint_limits.yaml new file mode 100644 index 0000000..3ae912e --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/arm_moveit_config/config/kinematics.yaml b/src/arm_moveit_config/config/kinematics.yaml new file mode 100644 index 0000000..2f6bb96 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/arm_moveit_config/config/moveit.rviz b/src/arm_moveit_config/config/moveit.rviz new file mode 100644 index 0000000..f31651e --- /dev/null +++ b/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 diff --git a/src/arm_moveit_config/config/moveit_controllers.yaml b/src/arm_moveit_config/config/moveit_controllers.yaml new file mode 100644 index 0000000..09dfa3d --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/arm_moveit_config/config/pilz_cartesian_limits.yaml b/src/arm_moveit_config/config/pilz_cartesian_limits.yaml new file mode 100644 index 0000000..b2997ca --- /dev/null +++ b/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 diff --git a/src/arm_moveit_config/config/ros2_controllers.yaml b/src/arm_moveit_config/config/ros2_controllers.yaml new file mode 100644 index 0000000..fec9baf --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/arm_moveit_config/config/six_axis_arm.ros2_control.xacro b/src/arm_moveit_config/config/six_axis_arm.ros2_control.xacro new file mode 100644 index 0000000..57b3e49 --- /dev/null +++ b/src/arm_moveit_config/config/six_axis_arm.ros2_control.xacro @@ -0,0 +1,56 @@ + + + + + + + + + mock_components/GenericSystem + + + + + ${initial_positions['joint_1']} + + + + + + + ${initial_positions['joint_2']} + + + + + + + ${initial_positions['joint_3']} + + + + + + + ${initial_positions['joint_4']} + + + + + + + ${initial_positions['joint_5']} + + + + + + + ${initial_positions['joint_6']} + + + + + + + diff --git a/src/arm_moveit_config/config/six_axis_arm.srdf b/src/arm_moveit_config/config/six_axis_arm.srdf new file mode 100644 index 0000000..c700bad --- /dev/null +++ b/src/arm_moveit_config/config/six_axis_arm.srdf @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/arm_moveit_config/config/six_axis_arm.urdf.xacro b/src/arm_moveit_config/config/six_axis_arm.urdf.xacro new file mode 100644 index 0000000..3f4ab5b --- /dev/null +++ b/src/arm_moveit_config/config/six_axis_arm.urdf.xacro @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/src/arm_moveit_config/launch/demo.launch.py b/src/arm_moveit_config/launch/demo.launch.py new file mode 100644 index 0000000..66f04a5 --- /dev/null +++ b/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) diff --git a/src/arm_moveit_config/launch/move_group.launch.py b/src/arm_moveit_config/launch/move_group.launch.py new file mode 100644 index 0000000..4c8410e --- /dev/null +++ b/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) diff --git a/src/arm_moveit_config/launch/moveit_rviz.launch.py b/src/arm_moveit_config/launch/moveit_rviz.launch.py new file mode 100644 index 0000000..8e4be11 --- /dev/null +++ b/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) diff --git a/src/arm_moveit_config/launch/rsp.launch.py b/src/arm_moveit_config/launch/rsp.launch.py new file mode 100644 index 0000000..44c4237 --- /dev/null +++ b/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) diff --git a/src/arm_moveit_config/launch/setup_assistant.launch.py b/src/arm_moveit_config/launch/setup_assistant.launch.py new file mode 100644 index 0000000..876c219 --- /dev/null +++ b/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) diff --git a/src/arm_moveit_config/launch/sim.launch.py b/src/arm_moveit_config/launch/sim.launch.py new file mode 100644 index 0000000..86079dc --- /dev/null +++ b/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, + ]) \ No newline at end of file diff --git a/src/arm_moveit_config/launch/spawn_controllers.launch.py b/src/arm_moveit_config/launch/spawn_controllers.launch.py new file mode 100644 index 0000000..9244d9b --- /dev/null +++ b/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) diff --git a/src/arm_moveit_config/launch/static_virtual_joint_tfs.launch.py b/src/arm_moveit_config/launch/static_virtual_joint_tfs.launch.py new file mode 100644 index 0000000..b3ceb23 --- /dev/null +++ b/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) diff --git a/src/arm_moveit_config/launch/warehouse_db.launch.py b/src/arm_moveit_config/launch/warehouse_db.launch.py new file mode 100644 index 0000000..f07d19e --- /dev/null +++ b/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) diff --git a/src/arm_moveit_config/package.xml b/src/arm_moveit_config/package.xml new file mode 100644 index 0000000..1836b40 --- /dev/null +++ b/src/arm_moveit_config/package.xml @@ -0,0 +1,52 @@ + + + + arm_moveit_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the six_axis_arm with the MoveIt Motion Planning Framework + + marcin + + BSD-3-Clause + + http://moveit.ros.org/ + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 + + marcin + + ament_cmake + + moveit_ros_move_group + moveit_kinematics + moveit_planners + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + tf2_ros + xacro + + + + controller_manager + moveit_configs_utils + moveit_ros_move_group + moveit_ros_visualization + moveit_ros_warehouse + moveit_setup_assistant + robot_state_publisher + robotarm_description + rviz2 + rviz_common + rviz_default_plugins + tf2_ros + warehouse_ros_mongo + xacro + + + + ament_cmake + + diff --git a/src/robotarm_description/CMakeLists.txt b/src/robotarm_description/CMakeLists.txt new file mode 100644 index 0000000..fac838c --- /dev/null +++ b/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( 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() diff --git a/src/robotarm_description/LICENSE b/src/robotarm_description/LICENSE new file mode 100644 index 0000000..574ef07 --- /dev/null +++ b/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. diff --git a/src/robotarm_description/config/ros2_controllers.yaml b/src/robotarm_description/config/ros2_controllers.yaml new file mode 100644 index 0000000..994c7b3 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/robotarm_description/config/sensors_3d.yaml b/src/robotarm_description/config/sensors_3d.yaml new file mode 100644 index 0000000..07ba0df --- /dev/null +++ b/src/robotarm_description/config/sensors_3d.yaml @@ -0,0 +1,2 @@ +sensors: + - {} \ No newline at end of file diff --git a/src/robotarm_description/package.xml b/src/robotarm_description/package.xml new file mode 100644 index 0000000..7780374 --- /dev/null +++ b/src/robotarm_description/package.xml @@ -0,0 +1,18 @@ + + + + robotarm_description + 0.0.0 + TODO: Package description + marcin + BSD-3-Clause + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/robotarm_description/urdf/robot_arm.urdf.xacro b/src/robotarm_description/urdf/robot_arm.urdf.xacro new file mode 100644 index 0000000..ecaf674 --- /dev/null +++ b/src/robotarm_description/urdf/robot_arm.urdf.xacro @@ -0,0 +1,255 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + + + 0.0 + + + + + + + + 0.0 + + + + + + + + 0.0 + + + + + + + + 0.0 + + + + + + + + 0.0 + + + + + + + + 0.0 + + + + + + + + $(find robotarm_description)/config/ros2_controllers.yaml + + + + \ No newline at end of file