diff --git a/README.md b/README.md index 9b77762..ba0cf9b 100644 --- a/README.md +++ b/README.md @@ -53,4 +53,10 @@ sudo pip3 install transforms3d --break-system-packages Спавн `.proto`: -Можно заготовить готовые `.proto` файлы, и потом случайно спавнить объект по такому принципу + создать Node который будет вызываться и спавнить этот объекты. Может быть шаблон куда потом подставятся данные через `.format()`. По такому же принципу спавн человека. Остается понять, только задать область спавна относительно робта \ No newline at end of file +Можно заготовить готовые `.proto` файлы, и потом случайно спавнить объект по такому принципу + создать Node который будет вызываться и спавнить этот объекты. Может быть шаблон куда потом подставятся данные через `.format()`. По такому же принципу спавн человека. Остается понять, только задать область спавна относительно робта + +Установка зависимостей для сборки пакетов: + +```bash +rosdep install --from-paths src --ignore-src -r -y --os=ubuntu:jammy +``` \ No newline at end of file diff --git a/src/iiwa_bringup/launch/digital_twin.launch.py b/src/iiwa_bringup/launch/digital_twin.launch.py index 0be8636..c1d04d0 100644 --- a/src/iiwa_bringup/launch/digital_twin.launch.py +++ b/src/iiwa_bringup/launch/digital_twin.launch.py @@ -26,7 +26,7 @@ def _runtime_setup(context, *args, **kwatgs): ) robot_description = converter.load_robot_description( - model_path=settings.digital_twin.description, + model_path=settings.robot.description, robot_name=settings.robot.name, xacro_args={ "initial_positions_file": settings.controller.moveit.initial_positions @@ -54,7 +54,7 @@ def _runtime_setup(context, *args, **kwatgs): ), launch_arguments={ "robot_name": str(settings.robot.name), - "description": str(settings.digital_twin.description), + "description": str(settings.robot.description), "world": str(settings.digital_twin.webots.world), "transform": str(settings.digital_twin.webots.transform), "rotation": str(settings.digital_twin.webots.rotation), @@ -67,7 +67,7 @@ def _runtime_setup(context, *args, **kwatgs): moveit_configs = ( MoveItConfigsBuilder("iiwa7", package_name="iiwa_config") .robot_description( - file_path=settings.digital_twin.description, + file_path=settings.robot.description, mappings={ "initial_positions_file": settings.controller.moveit.initial_positions }, diff --git a/src/iiwa_bringup/launch/iiwa.launch.py b/src/iiwa_bringup/launch/iiwa.launch.py index d5e500b..e263828 100644 --- a/src/iiwa_bringup/launch/iiwa.launch.py +++ b/src/iiwa_bringup/launch/iiwa.launch.py @@ -5,8 +5,6 @@ from launch.actions import ( IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler, - SetEnvironmentVariable, - # TimerAction, ) from launch.conditions import IfCondition from launch.event_handlers import OnProcessExit @@ -16,7 +14,6 @@ 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 @@ -79,29 +76,54 @@ def _runtime_setup(context, *args, **kwargs): setup += [rsp_node] - # gazebo spawn + # webots spawn if simulate: - gazebo_launch = IncludeLaunchDescription( + webots_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( [ FindPackageShare("iiwa_bringup"), - "launch", "supported", "gazebo.launch.py", + "launch", + "supported", + "webots_spawn.launch.py", ] ) ), - # TODO: добавить аргументы для transform и rotation и передать их из setting.yaml а также мир launch_arguments={ - "robot_name": settings.robot.name, - "world": str(get_package_share_directory("iiwa_description") + "/worlds/iiwa_world.sdf"), - "gazebo_config": str(get_package_share_directory("iiwa_config") + "/config/gazebo/gz_bridge.yaml"), - "simulate": "true", + "robot_name": str(settings.robot.name), + "description": str(settings.robot.description), + "world": str(settings.digital_twin.webots.world), + "transform": str(settings.digital_twin.webots.transform), + "rotation": str(settings.digital_twin.webots.rotation), + "controller_timer": str(settings.digital_twin.webots.controller_timer), + "controller": str(settings.controller.controller_path), + "initial_positions_file": str(settings.controller.moveit.initial_positions), }.items(), ) - setup += [gazebo_launch] + setup += [webots_launch] - # Controller launch + # Controller launch) + if simulate: + controller_args = { + "robot_name": settings.robot.name, + "description": description_path, + "initial_positions_file": settings.controller.moveit.initial_positions, + "controller_path": settings.controller.controller_path, + "simulate": "true", + "transform": str(settings.digital_twin.webots.transform), + "rotation": str(settings.digital_twin.webots.rotation), + "controller_timer": str(settings.digital_twin.webots.controller_timer), + } + else: + controller_args = { + "robot_name": settings.robot.name, + "description": description_path, + "initial_positions_file": settings.controller.moveit.initial_positions, + "controller_path": settings.controller.controller_path, + "simulate": "false", + } + controllers_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( @@ -113,13 +135,7 @@ def _runtime_setup(context, *args, **kwargs): ] ) ), - 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(), + launch_arguments=controller_args.items(), ) # Moveit launch @@ -206,15 +222,9 @@ def generate_launch_description(): 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, diff --git a/src/iiwa_bringup/launch/supported/controllers.launch.py b/src/iiwa_bringup/launch/supported/controllers.launch.py index b0135a3..295873c 100644 --- a/src/iiwa_bringup/launch/supported/controllers.launch.py +++ b/src/iiwa_bringup/launch/supported/controllers.launch.py @@ -4,6 +4,8 @@ from launch.substitutions import LaunchConfiguration from launch.event_handlers import OnProcessExit from launch.actions import RegisterEventHandler from launch_ros.actions import Node +from webots_ros2_driver.urdf_spawner import URDFSpawner + from iiwa_utils import converter @@ -11,27 +13,32 @@ from iiwa_utils import converter def _setup_controllers(context, *args, **kwargs): robot_name = LaunchConfiguration("robot_name").perform(context) description = LaunchConfiguration("description").perform(context) + transform = LaunchConfiguration("transform").perform(context) + rotation = LaunchConfiguration("rotation").perform(context) initial_positions_file = LaunchConfiguration("initial_positions_file").perform(context) + controller_timer = LaunchConfiguration("controller_timer").perform(context) controller_path = LaunchConfiguration("controller_path").perform(context) simulate = LaunchConfiguration("simulate").perform(context).lower() in ("true", "1", "yes") + xacro_args = {"initial_positions_file": initial_positions_file} + if simulate: + xacro_args["simulate"] = "true" + robot_description = converter.load_robot_description( model_path=description, robot_name=robot_name, - xacro_args={"initial_positions_file": initial_positions_file}, + xacro_args=xacro_args, ) - # СИМУЛЯЦИЯ (Gazebo) + # Webots if simulate: + tmo = ["--controller-manager-timeout", str(controller_timer)] + jsb = Node( package="controller_manager", executable="spawner", output="screen", - arguments=[ - "joint_state_broadcaster", - "--controller-manager", "/controller_manager", - "--controller-manager-timeout", "30", - ], + arguments=["joint_state_broadcaster"] + tmo, parameters=[{"use_sim_time": True}], ) @@ -39,24 +46,29 @@ def _setup_controllers(context, *args, **kwargs): package="controller_manager", executable="spawner", output="screen", - arguments=[ - "iiwa_arm_controller", - "--controller-manager", "/controller_manager", - "--controller-manager-timeout", "30", - ], + arguments=["iiwa_arm_controller"] + tmo, parameters=[{"use_sim_time": True}], ) - jtc_after_jsb = RegisterEventHandler( - OnProcessExit( - target_action=jsb, - on_exit=[jtc], - ) + torque_controller_spawner = Node( + package="controller_manager", + executable="spawner", + output="screen", + arguments=["forward_torque_controller", + "--inactive"] + tmo, + parameters=[{"use_sim_time": True}] ) - return [jsb, jtc_after_jsb] + spawner_urdf = URDFSpawner( + name=robot_name, + robot_description=robot_description, + translation=transform, + rotation=rotation, + ) - # РЕАЛЬНЫЙ РОБОТ (FRI) + return [jsb, jtc, torque_controller_spawner, spawner_urdf] + + # FRI else: ros2_control_node = Node( package="controller_manager", @@ -114,4 +126,4 @@ def _setup_controllers(context, *args, **kwargs): def generate_launch_description(): - return LaunchDescription([OpaqueFunction(function=_setup_controllers)]) \ No newline at end of file + return LaunchDescription([OpaqueFunction(function=_setup_controllers)]) diff --git a/src/iiwa_bringup/launch/supported/gazebo.launch.py b/src/iiwa_bringup/launch/supported/gazebo.launch.py deleted file mode 100644 index 50af1d8..0000000 --- a/src/iiwa_bringup/launch/supported/gazebo.launch.py +++ /dev/null @@ -1,90 +0,0 @@ -from ament_index_python import get_package_share_directory -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} --render-engine ogre", - "on_exit_shutdown": "true", - }.items(), - ) - - # Table Spawner - spawn_table = Node( - package="ros_gz_sim", - executable="create", - name="spawn_table", - output="screen", - arguments=[ - "-file", str( - get_package_share_directory("iiwa_description") - + "/urdf/table/table.urdf" - ), - "-name", "iiwa_table", - "-x", "0.0", - "-y", "0.0", - "-z", "0.0", - ], - ) - - # Robot Spawner - # TODO: добавить аргументы для transform и rotation - spawn_robot = Node( - package="ros_gz_sim", - executable="create", - name="spawn_iiwa7", - output="screen", - arguments=[ - "-topic", "/robot_description", - "-name", robot_name, - "-x", "-0.25", - "-y", "0.0", - "-z", "0.81" - ], - ) - - 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_table, - spawn_robot, - gz_bridge - ] - - -def generate_launch_description(): - return LaunchDescription([OpaqueFunction(function=_spawn_setup)]) diff --git a/src/iiwa_bringup/launch/supported/webots_controllers.launch.py b/src/iiwa_bringup/launch/supported/webots_controllers.launch.py deleted file mode 100644 index 05733c4..0000000 --- a/src/iiwa_bringup/launch/supported/webots_controllers.launch.py +++ /dev/null @@ -1,64 +0,0 @@ -from launch import LaunchDescription -from launch.actions import OpaqueFunction -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from webots_ros2_driver.urdf_spawner import URDFSpawner - -from iiwa_utils import converter - - -def _setup_controllers(context, *args, **kwargs): - robot_name = LaunchConfiguration("robot_name").perform(context) - description = LaunchConfiguration("description").perform(context) - transform = LaunchConfiguration("transform").perform(context) - rotation = LaunchConfiguration("rotation").perform(context) - controller_timer = LaunchConfiguration("controller_timer").perform(context) - initial_positions_file = LaunchConfiguration("initial_positions_file").perform( - context - ) - - robot_description = converter.load_robot_description( - model_path=description, - robot_name=robot_name, - xacro_args={"initial_positions_file": initial_positions_file}, - ) - - tmo = ["--controller-manager-timeout", str(controller_timer)] - - jsb = Node( - package="controller_manager", - executable="spawner", - output="screen", - arguments=["joint_state_broadcaster"] + tmo, - parameters=[{"use_sim_time": True}], - ) - - jtc = Node( - package="controller_manager", - executable="spawner", - output="screen", - arguments=["iiwa_arm_controller"] + tmo, - parameters=[{"use_sim_time": True}], - ) - - torque_controller_spawner = Node( - package="controller_manager", - executable="spawner", - output="screen", - arguments=["forward_torque_controller", - "--inactive"] + tmo, - parameters=[{"use_sim_time": True}] - ) - - spawner_urdf = URDFSpawner( - name=robot_name, - robot_description=robot_description, - translation=transform, - rotation=rotation, - ) - - return [jsb, jtc, torque_controller_spawner, spawner_urdf] - - -def generate_launch_description(): - return LaunchDescription([OpaqueFunction(function=_setup_controllers)]) diff --git a/src/iiwa_bringup/launch/supported/webots_spawn.launch.py b/src/iiwa_bringup/launch/supported/webots_spawn.launch.py index 0b82609..b9fd0cc 100644 --- a/src/iiwa_bringup/launch/supported/webots_spawn.launch.py +++ b/src/iiwa_bringup/launch/supported/webots_spawn.launch.py @@ -1,30 +1,32 @@ from launch import LaunchDescription from launch.actions import ( EmitEvent, - IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler, - TimerAction, ) -from launch.event_handlers import OnProcessExit, OnProcessStart +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.substitutions import FindPackageShare +from launch.substitutions import LaunchConfiguration from webots_ros2_driver.webots_controller import WebotsController from webots_ros2_driver.webots_launcher import WebotsLauncher +from iiwa_utils import converter + def _spawn_setup(context, *args, **kwargs): robot_name = LaunchConfiguration("robot_name").perform(context) description = LaunchConfiguration("description").perform(context) world = LaunchConfiguration("world").perform(context) - transform = LaunchConfiguration("transform").perform(context) - rotation = LaunchConfiguration("rotation").perform(context) - controller_timer = LaunchConfiguration("controller_timer").perform(context) controller = LaunchConfiguration("controller").perform(context) - initial_positions_file = LaunchConfiguration("initial_positions_file").perform( - context + initial_positions_file = LaunchConfiguration("initial_positions_file").perform(context) + + robot_description = converter.load_robot_description( + model_path=description, + robot_name=robot_name, + xacro_args={ + "simulate": "true", + "initial_positions_file": initial_positions_file, + }, ) webots = WebotsLauncher(world=world, ros2_supervisor=True) @@ -33,7 +35,7 @@ def _spawn_setup(context, *args, **kwargs): robot_name=robot_name, parameters=[ { - "robot_description": description, + "robot_description": robot_description, "use_sim_time": True, "set_robot_state_publisher": False, }, @@ -42,36 +44,6 @@ def _spawn_setup(context, *args, **kwargs): respawn=True, ) - controllers_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("iiwa_bringup"), - "launch", - "supported", - "webots_controllers.launch.py", - ] - ) - ), - launch_arguments={ - "robot_name": robot_name, - "description": description, - "transform": transform, - "rotation": rotation, - "controller_timer": controller_timer, - "initial_positions_file": initial_positions_file, - }.items(), - ) - - spawn_on_driver_start = RegisterEventHandler( - event_handler=OnProcessStart( - target_action=driver, - on_start=lambda evt, ctx: [ - TimerAction(period=5.0, actions=[controllers_launch]) - ], - ) - ) - shutdown_on_webots_exit = RegisterEventHandler( OnProcessExit(target_action=webots, on_exit=[EmitEvent(event=Shutdown())]) ) @@ -80,7 +52,6 @@ def _spawn_setup(context, *args, **kwargs): webots, webots._supervisor, driver, - spawn_on_driver_start, shutdown_on_webots_exit, ] diff --git a/src/iiwa_bringup/package.xml b/src/iiwa_bringup/package.xml index 1e5d3a3..f707264 100644 --- a/src/iiwa_bringup/package.xml +++ b/src/iiwa_bringup/package.xml @@ -3,13 +3,36 @@ iiwa_bringup 0.2.0 - Файлы запуска проектов + Launch-файлы для запуска KUKA iiwa7: симуляция Webots, реальный робот через FRI, MoveIt и RViz daniel Apache-2.0 ament_cmake + + iiwa_utils - + iiwa_config + iiwa_description + + + launch + launch_ros + robot_state_publisher + + + moveit_ros_move_group + moveit_configs_utils + rviz2 + + + controller_manager + joint_state_broadcaster + joint_trajectory_controller + forward_command_controller + + + webots_ros2_driver + ament_lint_auto ament_lint_common diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index 7f12971..d50cc52 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/src/iiwa_config/config/setting.yaml @@ -16,8 +16,6 @@ digital_twin: rviz: config: pkg://iiwa_config/config/rviz/rviz_moveit.rviz - description: pkg://iiwa_description/urdf/iiwa7_digital.urdf.xacro - controller: controller_path: pkg://iiwa_config/config/moveit/iiwa_controller.yaml diff --git a/src/iiwa_config/package.xml b/src/iiwa_config/package.xml index 1c5761d..122295e 100644 --- a/src/iiwa_config/package.xml +++ b/src/iiwa_config/package.xml @@ -3,7 +3,7 @@ iiwa_config 0.1.0 - Пакет с настройками цифрового двойника и физического робота + Конфигурационные файлы системы KUKA iiwa7: параметры MoveIt, контроллеров, кинематики и общих настроек daniel Apache-2.0 diff --git a/src/iiwa_controller/package.xml b/src/iiwa_controller/package.xml index 3f9004f..b894719 100644 --- a/src/iiwa_controller/package.xml +++ b/src/iiwa_controller/package.xml @@ -3,8 +3,8 @@ iiwa_controller 0.1.0 - Own controller for controlling the KUKA aiwa 7 collaborative robot using the FRI library - Daniell + Hardware interface для KUKA iiwa7: управление через FRI (Fast Robot Interface) в экосистеме ros2_control + daniel Apache-2.0 ament_cmake @@ -12,6 +12,7 @@ hardware_interface pluginlib rclcpp + rclcpp_lifecycle ament_lint_auto ament_lint_common diff --git a/src/iiwa_description/package.xml b/src/iiwa_description/package.xml index 0cb5ab6..f0071bf 100644 --- a/src/iiwa_description/package.xml +++ b/src/iiwa_description/package.xml @@ -3,12 +3,15 @@ iiwa_description 0.0.0 - Описание робота в urdf/xacro а также описание webots мира + URDF/XACRO описание робота KUKA iiwa7 и конфигурация мира Webots daniel Apache-2.0 ament_cmake + xacro + urdf + ament_lint_auto ament_lint_common diff --git a/src/iiwa_description/urdf/iiwa7.urdf.xacro b/src/iiwa_description/urdf/iiwa7.urdf.xacro index dfddf46..22e7644 100644 --- a/src/iiwa_description/urdf/iiwa7.urdf.xacro +++ b/src/iiwa_description/urdf/iiwa7.urdf.xacro @@ -15,26 +15,20 @@ - - - - $(find iiwa_config)/config/moveit/iiwa_controller.yaml - - ~/robot_description:=/robot_description - - - + + + + - gz_ros2_control/GazeboSimSystem + webots_ros2_control::Ros2ControlSystem diff --git a/src/iiwa_planning/package.xml b/src/iiwa_planning/package.xml index 99d03fa..730cff4 100644 --- a/src/iiwa_planning/package.xml +++ b/src/iiwa_planning/package.xml @@ -3,15 +3,22 @@ iiwa_planning 0.0.0 - TODO: Package description + Планирование движения KUKA iiwa7: C++ и Python узлы на основе MoveIt 2 (OMPL, Pilz, moveit_py) daniel Apache-2.0 ament_cmake ament_cmake_python - + rclcpp rclpy + moveit_ros_planning_interface + moveit_core + moveit_ros_planning + moveit_msgs + geometry_msgs + moveit_py + tf_transformations ament_lint_auto ament_lint_common diff --git a/src/iiwa_utils/iiwa_utils/setting_loader.py b/src/iiwa_utils/iiwa_utils/setting_loader.py index a934ad6..0190afb 100644 --- a/src/iiwa_utils/iiwa_utils/setting_loader.py +++ b/src/iiwa_utils/iiwa_utils/setting_loader.py @@ -34,8 +34,6 @@ class RvizCfg: class DigitalTwinCfg: webots: WebotsCfg rviz: RvizCfg - description: str - @dataclass(frozen=True) class MoveitCfg: @@ -175,8 +173,7 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings: digital_twin = DigitalTwinCfg( webots=webots, - rviz=rviz, - description=resolve_path(str(require(dt_raw, "description")), settings_dir), + rviz=rviz ) # controller, moveit @@ -212,7 +209,6 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings: assert_file(s.robot.description, "robot.description") assert_file(s.digital_twin.webots.world, "digital_twin.webots.world") assert_file(s.digital_twin.rviz.config, "digital_twin.rviz.config") - assert_file(s.digital_twin.description, "digital_twin.description") assert_file(s.controller.controller_path, "controller.controller_path") assert_file(s.controller.moveit.srdf, "controller.moveit.srdf") diff --git a/src/iiwa_utils/package.xml b/src/iiwa_utils/package.xml index 05c6e26..03e4613 100644 --- a/src/iiwa_utils/package.xml +++ b/src/iiwa_utils/package.xml @@ -3,10 +3,16 @@ iiwa_utils 0.1.0 - Вспомогательные файлы для работы всей системы + Утилиты для работы системы iiwa7: загрузка конфигурации, обработка URDF/XACRO, спавн объектов в Webots daniel Apache-2.0 + rclpy + xacro + ament_index_python + webots_ros2_msgs + python3-yaml + ament_copyright ament_flake8 ament_pep257