diff --git a/src/iiwa_bringup/launch/supported/controllers.launch.py b/src/iiwa_bringup/launch/supported/controllers.launch.py new file mode 100644 index 0000000..b0135a3 --- /dev/null +++ b/src/iiwa_bringup/launch/supported/controllers.launch.py @@ -0,0 +1,117 @@ +from launch import LaunchDescription +from launch.actions import OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch.event_handlers import OnProcessExit +from launch.actions import RegisterEventHandler +from launch_ros.actions import Node + +from iiwa_utils import converter + + +def _setup_controllers(context, *args, **kwargs): + robot_name = LaunchConfiguration("robot_name").perform(context) + description = LaunchConfiguration("description").perform(context) + initial_positions_file = LaunchConfiguration("initial_positions_file").perform(context) + controller_path = LaunchConfiguration("controller_path").perform(context) + simulate = LaunchConfiguration("simulate").perform(context).lower() in ("true", "1", "yes") + + robot_description = converter.load_robot_description( + model_path=description, + robot_name=robot_name, + xacro_args={"initial_positions_file": initial_positions_file}, + ) + + # СИМУЛЯЦИЯ (Gazebo) + if simulate: + jsb = Node( + package="controller_manager", + executable="spawner", + output="screen", + arguments=[ + "joint_state_broadcaster", + "--controller-manager", "/controller_manager", + "--controller-manager-timeout", "30", + ], + parameters=[{"use_sim_time": True}], + ) + + jtc = Node( + package="controller_manager", + executable="spawner", + output="screen", + arguments=[ + "iiwa_arm_controller", + "--controller-manager", "/controller_manager", + "--controller-manager-timeout", "30", + ], + parameters=[{"use_sim_time": True}], + ) + + jtc_after_jsb = RegisterEventHandler( + OnProcessExit( + target_action=jsb, + on_exit=[jtc], + ) + ) + + return [jsb, jtc_after_jsb] + + # РЕАЛЬНЫЙ РОБОТ (FRI) + else: + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + output="screen", + parameters=[ + {"robot_description": robot_description}, + controller_path, + ], + ) + + jsb = Node( + package="controller_manager", + executable="spawner", + output="screen", + arguments=[ + "joint_state_broadcaster", + "--controller-manager", "/controller_manager", + ], + ) + + jtc = Node( + package="controller_manager", + executable="spawner", + output="screen", + arguments=[ + "iiwa_arm_controller", + "--controller-manager", "/controller_manager", + ], + ) + + torque_controller = Node( + package="controller_manager", + executable="spawner", + output="screen", + arguments=[ + "forward_torque_controller", + "--controller-manager", "/controller_manager", + "--inactive", + ], + ) + + jtc_after_jsb = RegisterEventHandler( + OnProcessExit( + target_action=jsb, + on_exit=[jtc, torque_controller], + ) + ) + + return [ + ros2_control_node, + jsb, + jtc_after_jsb, + ] + + +def generate_launch_description(): + return LaunchDescription([OpaqueFunction(function=_setup_controllers)]) \ No newline at end of file diff --git a/src/iiwa_bringup/launch/supported/gazebo.launch.py b/src/iiwa_bringup/launch/supported/gazebo.launch.py new file mode 100644 index 0000000..5c4c321 --- /dev/null +++ b/src/iiwa_bringup/launch/supported/gazebo.launch.py @@ -0,0 +1,63 @@ +from launch import LaunchDescription +from launch.actions import ( + IncludeLaunchDescription, + OpaqueFunction, +) +from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def _spawn_setup(context, *args, **kwargs): + robot_name = LaunchConfiguration("robot_name").perform(context) + world = LaunchConfiguration("world").perform(context) + gazebo_config = LaunchConfiguration("gazebo_config").perform(context) + simulate = LaunchConfiguration("simulate").perform(context).lower() in ("true", "1", "yes") + # transform = LaunchConfiguration("transform").perform(context) + + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("ros_gz_sim"), + "launch", + "gz_sim.launch.py", + ] + ) + ), + launch_arguments={ + "gz_args": f"-r {world}", + "on_exit_shutdown": "true", + }.items(), + ) + + # TODO: добавить аргументы для transform и rotation + spawn_robot = Node( + package="ros_gz_sim", + executable="create", + name="spawn_iiwa7", + output="screen", + arguments=[ + "-topic", "/robot_description", + "-name", robot_name, + "-z", "0.0" + ], + ) + + gz_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="gz_bridge", + output="screen", + parameters=[{ + "config_file": gazebo_config, + "use_sim_time": simulate + }], + ) + + return [gazebo, spawn_robot, gz_bridge] + + +def generate_launch_description(): + return LaunchDescription([OpaqueFunction(function=_spawn_setup)]) \ No newline at end of file diff --git a/src/iiwa_bringup/launch/supported/iiwa_controllers.launch.py b/src/iiwa_bringup/launch/supported/iiwa_controllers.launch.py index 86d678b..9be7d59 100644 --- a/src/iiwa_bringup/launch/supported/iiwa_controllers.launch.py +++ b/src/iiwa_bringup/launch/supported/iiwa_controllers.launch.py @@ -33,7 +33,6 @@ def _setup_controllers(context, *args, **kwargs): ], ) - joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", diff --git a/src/iiwa_bringup/launch/universal.launch.py b/src/iiwa_bringup/launch/universal.launch.py new file mode 100644 index 0000000..e233d16 --- /dev/null +++ b/src/iiwa_bringup/launch/universal.launch.py @@ -0,0 +1,223 @@ +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + EmitEvent, + IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, + SetEnvironmentVariable, + # TimerAction, +) +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder +from ament_index_python.packages import get_package_share_directory + +from iiwa_utils import converter, setting_loader + + +def _runtime_setup(context, *args, **kwargs): + setup = [] + + # Настройка параметров + simulate = LaunchConfiguration("simulate").perform(context) in ("true", "1", "yes") + + settings = setting_loader.build_settings( + settings_path=LaunchConfiguration("setting").perform(context), + check_files=True, + ) + + joint_limits_ros2 = converter.wrap_for_ros2_params( + settings.controller.moveit.joint_limits, + "robot_description_planning", + ) + kinematics_ros2 = converter.wrap_for_ros2_params( + settings.controller.moveit.kinematics, + "robot_description_kinematics", + ) + + description_path = settings.robot.description + + if simulate: + xacro_args = { + "initial_positions_file": settings.controller.moveit.initial_positions, + "simulate": "true", + } + use_sim_time = True + else: + xacro_args = { + "initial_positions_file": settings.controller.moveit.initial_positions, + "robot_ip": settings.robot.ip, + "fri_port": str(settings.robot.port), + "simulate": "false", + "command_mode": settings.robot.command_mode, + } + use_sim_time = False + + robot_description = converter.load_robot_description( + model_path=description_path, + robot_name=settings.robot.name, + xacro_args=xacro_args, + ) + + # Вызов нод + rsp_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[ + {"robot_description": robot_description}, + {"use_sim_time": use_sim_time}, + ], + ) + + setup += [rsp_node] + + # gazebo spawn + if simulate: + gazebo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("iiwa_bringup"), + "launch", "supported", "gazebo.launch.py", + ] + ) + ), + # TODO: добавить аргументы для transform и rotation и передать их из setting.yaml а также мир + launch_arguments={ + "robot_name": settings.robot.name, + "world": "empty.sdf", + "gazebo_config": str(get_package_share_directory("iiwa_config") + "/config/gazebo/gz_bridge.yaml"), + "simulate": "true", + }.items(), + ) + + setup += [gazebo_launch] + + # Controller launch + controllers_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("iiwa_bringup"), + "launch", + "supported", + "controllers.launch.py", + ] + ) + ), + launch_arguments={ + "robot_name": settings.robot.name, + "description": description_path, + "initial_positions_file": settings.controller.moveit.initial_positions, + # "simulate": str(simulate), + "controller_path": settings.controller.controller_path, + }.items(), + ) + + # Moveit launch + moveit_configs = ( + MoveItConfigsBuilder("iiwa7", package_name="iiwa_config") + .robot_description( + file_path=description_path, + mappings={ + "initial_positions_file": settings.controller.moveit.initial_positions + }, + ) + .robot_description_semantic(file_path=settings.controller.moveit.srdf) + .robot_description_kinematics(file_path=settings.controller.moveit.kinematics) + .joint_limits(file_path=settings.controller.moveit.joint_limits) + .pilz_cartesian_limits(file_path=settings.controller.moveit.pilz_limits) + .trajectory_execution(file_path=settings.controller.moveit.moveit_controllers) + .moveit_cpp(file_path=settings.controller.moveit.moveit_cpp) + .to_moveit_configs() + ) + + move_group = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + moveit_configs.to_dict(), + {"robot_description": robot_description}, + {"use_sim_time": use_sim_time}, + ], + ) + + # Rviz launch + rviz_launch = Node( + condition=IfCondition(LaunchConfiguration("rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", settings.digital_twin.rviz.config], + output="log", + parameters=[ + moveit_configs.robot_description, + moveit_configs.robot_description_semantic, + moveit_configs.planning_pipelines, + joint_limits_ros2, + kinematics_ros2, + {"use_sim_time": use_sim_time}, + ], + ) + + shutdown_on_rviz_exit = RegisterEventHandler( + OnProcessExit( + target_action=rviz_launch, + on_exit=[EmitEvent(event=Shutdown())], + ) + ) + + setup += [ + controllers_launch, + move_group, + rviz_launch, + shutdown_on_rviz_exit + ] + + return setup + +def generate_launch_description(): + declare_simulate = DeclareLaunchArgument( + name="simulate", + default_value="false", + description="true = Gazebo симуляция, false = реальный робот через FRI", + ) + + declare_rviz = DeclareLaunchArgument( + name="rviz", + default_value="false", + description="true = запустить RViz", + ) + + declare_setting = DeclareLaunchArgument( + name="setting", + default_value=PathJoinSubstitution( + [FindPackageShare("iiwa_config"), "config", "setting.yaml"] + ), + description="Путь к файлу настроек", + ) + + gz_resource_path = SetEnvironmentVariable( + name="GZ_SIM_RESOURCE_PATH", + value=get_package_share_directory("iiwa_description") + "/..", + ) + + runtime_setup = OpaqueFunction(function=_runtime_setup) + + return LaunchDescription([ + gz_resource_path, + declare_simulate, + declare_rviz, + declare_setting, + runtime_setup, + ]) + diff --git a/src/iiwa_config/config/gazebo/gz_bridge.yaml b/src/iiwa_config/config/gazebo/gz_bridge.yaml new file mode 100644 index 0000000..4f23c22 --- /dev/null +++ b/src/iiwa_config/config/gazebo/gz_bridge.yaml @@ -0,0 +1,29 @@ +# Формат каждой записи: +# ros_topic_name: имя топика в ROS2 +# gz_topic_name: имя топика в Gazebo (можно опустить если совпадает) +# ros_type_name: тип сообщения ROS2 +# gz_type_name: тип сообщения Gazebo +# direction: GZ_TO_ROS / ROS_TO_GZ / BIDIRECTIONAL + +- ros_topic_name: /clock + gz_topic_name: /clock + ros_type_name: rosgraph_msgs/msg/Clock + gz_type_name: gz.msgs.Clock + direction: GZ_TO_ROS + lazy: false + +# Состояния джойнтов из Gazebo в ROS2 +- ros_topic_name: /joint_states + gz_topic_name: /world/empty/model/iiwa7/joint_state + ros_type_name: sensor_msgs/msg/JointState + gz_type_name: gz.msgs.Model + direction: GZ_TO_ROS + lazy: false + +# TF из Gazebo в ROS2 (поза модели в мире) +- ros_topic_name: /tf + gz_topic_name: /model/iiwa7/pose + ros_type_name: tf2_msgs/msg/TFMessage + gz_type_name: gz.msgs.Pose_V + direction: GZ_TO_ROS + lazy: false \ No newline at end of file diff --git a/src/iiwa_config/config/moveit/iiwa7.srdf b/src/iiwa_config/config/moveit/iiwa7.srdf index 9fba6d1..b26eb15 100644 --- a/src/iiwa_config/config/moveit/iiwa7.srdf +++ b/src/iiwa_config/config/moveit/iiwa7.srdf @@ -43,9 +43,6 @@ - - - diff --git a/src/iiwa_description/urdf/gripper/___gripper.urdf b/src/iiwa_description/urdf/gripper/___gripper.urdf index 9726af4..e8d5c08 100644 --- a/src/iiwa_description/urdf/gripper/___gripper.urdf +++ b/src/iiwa_description/urdf/gripper/___gripper.urdf @@ -60,82 +60,8 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -207,7 +133,7 @@ - + @@ -220,42 +146,12 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + @@ -264,7 +160,7 @@ - + @@ -273,7 +169,7 @@ - + diff --git a/src/iiwa_description/urdf/gripper/gripper.xacro b/src/iiwa_description/urdf/gripper/gripper.xacro index 7c70a9e..1bcfc8c 100644 --- a/src/iiwa_description/urdf/gripper/gripper.xacro +++ b/src/iiwa_description/urdf/gripper/gripper.xacro @@ -1,16 +1,13 @@ - - - + - + \ No newline at end of file diff --git a/src/iiwa_description/urdf/gripper/gripper_joints.xacro b/src/iiwa_description/urdf/gripper/gripper_joints.xacro index 855facb..d7063de 100644 --- a/src/iiwa_description/urdf/gripper/gripper_joints.xacro +++ b/src/iiwa_description/urdf/gripper/gripper_joints.xacro @@ -2,9 +2,10 @@ + - + @@ -16,47 +17,25 @@ lower="-0.007" upper="0.0" effort="50.0" velocity="0.05"/> - - - - - - - \ No newline at end of file diff --git a/src/iiwa_description/urdf/gripper/gripper_links.xacro b/src/iiwa_description/urdf/gripper/gripper_links.xacro index 8c78431..630b8a6 100644 --- a/src/iiwa_description/urdf/gripper/gripper_links.xacro +++ b/src/iiwa_description/urdf/gripper/gripper_links.xacro @@ -1,10 +1,7 @@ - - + @@ -19,73 +16,37 @@ - + r="${gripper_dark_r}" g="${gripper_dark_g}" b="${gripper_dark_b}" a="1.0"/> - + r="${gripper_alum_r}" g="${gripper_alum_g}" b="${gripper_alum_b}" a="1.0"/> - + r="${gripper_alum_r}" g="${gripper_alum_g}" b="${gripper_alum_b}" a="1.0"/> - - - - - - - - + r="${gripper_finger_r}" g="${gripper_finger_g}" b="${gripper_finger_b}" a="1.0"/> + r="${gripper_finger_r}" g="${gripper_finger_g}" b="${gripper_finger_b}" a="1.0"/> + r="${gripper_finger_r}" g="${gripper_finger_g}" b="${gripper_finger_b}" a="1.0"/> - + \ No newline at end of file diff --git a/src/iiwa_description/urdf/gripper/gripper_macros.xacro b/src/iiwa_description/urdf/gripper/gripper_macros.xacro index 30feb9c..1aed3bd 100644 --- a/src/iiwa_description/urdf/gripper/gripper_macros.xacro +++ b/src/iiwa_description/urdf/gripper/gripper_macros.xacro @@ -1,15 +1,11 @@ - - + mesh visual_xyz visual_rpy + r g b a"> @@ -27,14 +23,33 @@ - + - + - + + + + ${r} ${g} ${b} ${a} + ${r} ${g} ${b} ${a} + 0.3 0.3 0.3 1.0 + + + + + + 1.01.0 + + + 1000000.0100.0 + + + + + @@ -67,4 +82,4 @@ - + \ No newline at end of file diff --git a/src/iiwa_description/urdf/gripper/gripper_meshes.xacro b/src/iiwa_description/urdf/gripper/gripper_meshes.xacro index 2033b16..194fce7 100644 --- a/src/iiwa_description/urdf/gripper/gripper_meshes.xacro +++ b/src/iiwa_description/urdf/gripper/gripper_meshes.xacro @@ -7,7 +7,6 @@ - \ No newline at end of file diff --git a/src/iiwa_description/urdf/iiwa7_digital.urdf.xacro b/src/iiwa_description/urdf/iiwa7_digital.urdf.xacro index 41654a3..9d77780 100644 --- a/src/iiwa_description/urdf/iiwa7_digital.urdf.xacro +++ b/src/iiwa_description/urdf/iiwa7_digital.urdf.xacro @@ -4,6 +4,13 @@ + + + + + + + @@ -15,24 +22,49 @@ - + + + + $(find iiwa_config)/config/moveit/iiwa_controller.yaml + + ~/robot_description:=/robot_description + + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + - - webots_ros2_control::Ros2ControlSystem + iiwa_controller/IIWAHardwareInterface + + $(arg robot_ip) + $(arg fri_port) + $(arg simulate) + $(arg command_mode) + - + + + -2.97 2.97 - - -320 - 320 - ${initial_positions['joint1']} @@ -45,10 +77,6 @@ -2.09 2.09 - - -320 - 320 - ${initial_positions['joint2']} @@ -61,10 +89,6 @@ -2.97 2.97 - - -320 - 320 - ${initial_positions['joint3']} @@ -77,10 +101,6 @@ -2.09 2.09 - - -320 - 320 - ${initial_positions['joint4']} @@ -93,10 +113,6 @@ -2.97 2.97 - - -320 - 320 - ${initial_positions['joint5']} @@ -109,10 +125,6 @@ -2.09 2.09 - - -320 - 320 - ${initial_positions['joint6']} @@ -125,10 +137,6 @@ -3.05 3.05 - - -320 - 320 - ${initial_positions['joint7']} @@ -136,16 +144,14 @@ - - + - \ No newline at end of file diff --git a/src/iiwa_description/urdf/iiwa7_fri.urdf.xacro b/src/iiwa_description/urdf/iiwa7_fri.urdf.xacro index f9b8096..e6eba36 100644 --- a/src/iiwa_description/urdf/iiwa7_fri.urdf.xacro +++ b/src/iiwa_description/urdf/iiwa7_fri.urdf.xacro @@ -1,13 +1,11 @@ - - + - - - - iiwa_controller/IIWAHardwareInterface + - $(arg robot_ip) - $(arg fri_port) - $(arg simulate) - $(arg command_mode) + + + $(find iiwa_config)/config/moveit/iiwa_controller.yaml + + ~/robot_description:=/robot_description + + + - + + + gz_ros2_control/GazeboSimSystem + - - - -2.97 - 2.97 - - - -320 - 320 - - - ${initial_positions['joint1']} - - - - + + + -2.97 + 2.97 + + + ${initial_positions['joint1']} + + + + - - - -2.09 - 2.09 - - - -320 - 320 - - - ${initial_positions['joint2']} - - - - + + + -2.09 + 2.09 + + + ${initial_positions['joint2']} + + + + - - - -2.97 - 2.97 - - - -320 - 320 - - - ${initial_positions['joint3']} - - - - + + + -2.97 + 2.97 + + + ${initial_positions['joint3']} + + + + - - - -2.09 - 2.09 - - - -320 - 320 - - - ${initial_positions['joint4']} - - - - + + + -2.09 + 2.09 + + + ${initial_positions['joint4']} + + + + - - - -2.97 - 2.97 - - - -320 - 320 - - - ${initial_positions['joint5']} - - - - + + + -2.97 + 2.97 + + + ${initial_positions['joint5']} + + + + - - - -2.09 - 2.09 - - - -320 - 320 - - - ${initial_positions['joint6']} - - - - + + + -2.09 + 2.09 + + + ${initial_positions['joint6']} + + + + - - - -3.05 - 3.05 - - - -320 - 320 - - - ${initial_positions['joint7']} - - - - + + + -3.05 + 3.05 + + + ${initial_positions['joint7']} + + + + - + + + + 0.0 + + + + + + + + + + + + + iiwa_controller/IIWAHardwareInterface + $(arg robot_ip) + $(arg fri_port) + false + $(arg command_mode) + + + + + -2.97 + 2.97 + + + -320 + 320 + + + ${initial_positions['joint1']} + + + + + + + + -2.09 + 2.09 + + + -320 + 320 + + + ${initial_positions['joint2']} + + + + + + + + -2.97 + 2.97 + + + -320 + 320 + + + ${initial_positions['joint3']} + + + + + + + + -2.09 + 2.09 + + + -320 + 320 + + + ${initial_positions['joint4']} + + + + + + + + -2.97 + 2.97 + + + -320 + 320 + + + ${initial_positions['joint5']} + + + + + + + + -2.09 + 2.09 + + + -320 + 320 + + + ${initial_positions['joint6']} + + + + + + + + -3.05 + 3.05 + + + -320 + 320 + + + ${initial_positions['joint7']} + + + + + + + + \ No newline at end of file diff --git a/src/iiwa_description/urdf/macros.xacro b/src/iiwa_description/urdf/macros.xacro index ec8f866..6f560f5 100644 --- a/src/iiwa_description/urdf/macros.xacro +++ b/src/iiwa_description/urdf/macros.xacro @@ -27,6 +27,17 @@ + + + + + ${color_r * 0.7} ${color_g * 0.7} ${color_b * 0.7} ${color_a} + ${color_r} ${color_g} ${color_b} ${color_a} + 0.3 0.3 0.3 1.0 + 0 0 0 1 + + + diff --git a/src/iiwa_utils/iiwa_utils/converter.py b/src/iiwa_utils/iiwa_utils/converter.py index 67dce82..8caf5f2 100644 --- a/src/iiwa_utils/iiwa_utils/converter.py +++ b/src/iiwa_utils/iiwa_utils/converter.py @@ -1,3 +1,6 @@ +import yaml +import tempfile + from pathlib import Path from typing import Dict, Optional, Union @@ -23,3 +26,13 @@ def load_robot_description( return model_path.read_text(encoding="utf-8") raise FileNotFoundError(f"Supported file formats: .xacro/.urdf, got: {model_path}") + + +def wrap_for_ros2_params(yaml_path: str, namespace: str) -> str: + with open(yaml_path, "r") as f: + data = yaml.safe_load(f) + wrapped = {namespace: {"ros__parameters": data}} + tmp = tempfile.NamedTemporaryFile(mode="w", suffix=".yaml", delete=False) + yaml.dump(wrapped, tmp, default_flow_style=False) + tmp.close() + return tmp.name \ No newline at end of file diff --git a/src/iiwa_utils/iiwa_utils/setting_loader.py b/src/iiwa_utils/iiwa_utils/setting_loader.py index dd4322e..a934ad6 100644 --- a/src/iiwa_utils/iiwa_utils/setting_loader.py +++ b/src/iiwa_utils/iiwa_utils/setting_loader.py @@ -229,3 +229,5 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings: assert_file(s.controller.moveit.moveit_cpp, "controller.moveit.moveit_cpp") return s + +