diff --git a/src/iiwa_bringup/config/iiwa7.srdf b/src/iiwa_bringup/config/iiwa7.srdf new file mode 100644 index 0000000..a640dd5 --- /dev/null +++ b/src/iiwa_bringup/config/iiwa7.srdf @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/iiwa_bringup/config/iiwa_controller.yaml b/src/iiwa_bringup/config/iiwa_controller.yaml index bb55e8d..7ad5862 100644 --- a/src/iiwa_bringup/config/iiwa_controller.yaml +++ b/src/iiwa_bringup/config/iiwa_controller.yaml @@ -8,7 +8,6 @@ controller_manager: iiwa_arm_controller: type: "joint_trajectory_controller/JointTrajectoryController" - iiwa_arm_controller: ros__parameters: joints: @@ -28,3 +27,4 @@ iiwa_arm_controller: allow_partial_joints_goal: false interpolate_from_desired_state: true + allow_nonzero_velocity_at_trajectory_end: true diff --git a/src/iiwa_bringup/config/initial_positions.yaml b/src/iiwa_bringup/config/initial_positions.yaml new file mode 100644 index 0000000..e2bae96 --- /dev/null +++ b/src/iiwa_bringup/config/initial_positions.yaml @@ -0,0 +1,10 @@ +# Default initial positions for iiwa7's ros2_control fake system + +initial_positions: + joint1: 0 + joint2: 0 + joint3: 0 + joint4: 0 + joint5: 0 + joint6: 0 + joint7: 0 \ No newline at end of file diff --git a/src/iiwa_bringup/config/joint_limits.yaml b/src/iiwa_bringup/config/joint_limits.yaml new file mode 100644 index 0000000..74c4f9d --- /dev/null +++ b/src/iiwa_bringup/config/joint_limits.yaml @@ -0,0 +1,45 @@ +# 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: + joint1: + has_velocity_limits: true + max_velocity: 1.71 + has_acceleration_limits: true + max_acceleration: 3.0 + joint2: + has_velocity_limits: true + max_velocity: 1.71 + has_acceleration_limits: true + max_acceleration: 3.0 + joint3: + has_velocity_limits: true + max_velocity: 1.75 + has_acceleration_limits: true + max_acceleration: 3.0 + joint4: + has_velocity_limits: true + max_velocity: 2.27 + has_acceleration_limits: true + max_acceleration: 3.0 + joint5: + has_velocity_limits: true + max_velocity: 2.4399999999999999 + has_acceleration_limits: true + max_acceleration: 3.0 + joint6: + has_velocity_limits: true + max_velocity: 3.1400000000000001 + has_acceleration_limits: true + max_acceleration: 3.0 + joint7: + has_velocity_limits: true + max_velocity: 3.1400000000000001 + has_acceleration_limits: true + max_acceleration: 3.0 \ No newline at end of file diff --git a/src/iiwa_bringup/config/kinematics.yaml b/src/iiwa_bringup/config/kinematics.yaml new file mode 100644 index 0000000..2f6bb96 --- /dev/null +++ b/src/iiwa_bringup/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/iiwa_bringup/config/moveit_controllers.yaml b/src/iiwa_bringup/config/moveit_controllers.yaml new file mode 100644 index 0000000..ef718d6 --- /dev/null +++ b/src/iiwa_bringup/config/moveit_controllers.yaml @@ -0,0 +1,20 @@ +# MoveIt uses this configuration for controller management + +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - iiwa_arm_controller + + iiwa_arm_controller: + type: FollowJointTrajectory + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint7 + action_ns: follow_joint_trajectory + default: true \ No newline at end of file diff --git a/src/iiwa_bringup/config/pilz_cartesian_limits.yaml b/src/iiwa_bringup/config/pilz_cartesian_limits.yaml new file mode 100644 index 0000000..b2997ca --- /dev/null +++ b/src/iiwa_bringup/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/iiwa_bringup/launch/digital_twin.launch.py b/src/iiwa_bringup/launch/digital_twin.launch.py index 6a035df..549b387 100644 --- a/src/iiwa_bringup/launch/digital_twin.launch.py +++ b/src/iiwa_bringup/launch/digital_twin.launch.py @@ -1,13 +1,19 @@ from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, + EmitEvent, IncludeLaunchDescription, OpaqueFunction, + RegisterEventHandler, ) +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 iiwa_bringup.utils import converter @@ -15,20 +21,38 @@ PACKAGE = "iiwa_bringup" DESCRIPTION_PKG = "iiwa_description" +def _arg(name: str, default, desc: str): + """Short helper to declare a launch argument.""" + return DeclareLaunchArgument(name=name, default_value=default, description=desc) + + +def _share_file(pkg_arg_name: str, *relpath: str): + """Join / where is FindPackageShare(LaunchConfiguration(pkg_arg_name)).""" + return PathJoinSubstitution([FindPackageShare(LaunchConfiguration(pkg_arg_name)), *relpath]) + + def _runtime_setup(context, *args, **kwatgs): setup = [] - model_path = LaunchConfiguration("model").perform(context) robot_name = LaunchConfiguration("robot_name").perform(context) world_path = LaunchConfiguration("world").perform(context) - rviz_status = LaunchConfiguration("rviz").perform(context).lower() in ["true", "1", "yes",] - transform = LaunchConfiguration("transform").perform(context) rotation = LaunchConfiguration("rotation").perform(context) timer = LaunchConfiguration("controller_timer").perform(context) + xacro_file = LaunchConfiguration("xacro_file").perform(context) + srdf_file = LaunchConfiguration("srdf_file").perform(context) + + # ros2_controllers_file = LaunchConfiguration("controller").perform(context) + initial_positions_file = LaunchConfiguration("initial_positions_file").perform(context) + + kinematics_yaml = LaunchConfiguration("kinematics_yaml").perform(context) + joint_limits_yaml = LaunchConfiguration("joint_limits_yaml").perform(context) + pilz_limits_yaml = LaunchConfiguration("pilz_limits_yaml").perform(context) + moveit_controllers_yaml = LaunchConfiguration("moveit_controllers_yaml").perform(context) + robot_description = converter.load_robot_description( - model_path=model_path, robot_name=robot_name + model_path=xacro_file, robot_name=robot_name ) rsp_node = Node( @@ -36,7 +60,8 @@ def _runtime_setup(context, *args, **kwatgs): executable="robot_state_publisher", name="robot_state_publisher", output="screen", - parameters=[{"robot_description": robot_description, "use_sim_time": True}], + parameters=[{"robot_description": robot_description, + "use_sim_time": False}], ) webots_launch = IncludeLaunchDescription( @@ -52,7 +77,7 @@ def _runtime_setup(context, *args, **kwatgs): ), launch_arguments={ "robot_name": robot_name, - "model": model_path, + "xacro_file": xacro_file, "world": world_path, "transform": transform, "rotation": rotation, @@ -60,91 +85,176 @@ def _runtime_setup(context, *args, **kwatgs): }.items(), ) - setup += [rsp_node, webots_launch] + moveit_configs = ( + MoveItConfigsBuilder("iiwa7", package_name=LaunchConfiguration("config_pkg").perform(context)) + .robot_description(file_path=xacro_file, mappings={ + "initial_positions_file": initial_positions_file, + }) + .robot_description_semantic(file_path=srdf_file) + .robot_description_kinematics(file_path=kinematics_yaml) + .joint_limits(file_path=joint_limits_yaml) + .pilz_cartesian_limits(file_path=pilz_limits_yaml) + .trajectory_execution(file_path=moveit_controllers_yaml) + .to_moveit_configs() + ) - if rviz_status: - rviz_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare(PACKAGE), "launch", "supported", "rviz.launch.py"] - ) - ), - launch_arguments={"package_name": PACKAGE}.items(), - ) + move_group = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + moveit_configs.to_dict(), + robot_description, + ], + ) - setup.append(rviz_launch) + rviz_config = PathJoinSubstitution( + [FindPackageShare(PACKAGE), "config", "rviz_iiwa.rviz"] + ) + + rviz_launch = Node( + condition=IfCondition(LaunchConfiguration("rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config], + output="log", + parameters=[ + moveit_configs.robot_description, + moveit_configs.robot_description_semantic, + moveit_configs.robot_description_kinematics, + moveit_configs.planning_pipelines, + moveit_configs.joint_limits, + ], + ) + + shutdown_on_rviz_exit = RegisterEventHandler( + OnProcessExit(target_action=rviz_launch, + on_exit=[EmitEvent(event=Shutdown())]) + ) + + setup += [rsp_node, + webots_launch, + move_group, + rviz_launch, + shutdown_on_rviz_exit + ] return setup def generate_launch_description(): # Объявление аргументов командной строки - declare_model_arg = DeclareLaunchArgument( - name="model", - default_value=PathJoinSubstitution( - [FindPackageShare(DESCRIPTION_PKG), "urdf", "iiwa7.urdf.xacro"] - ), - description="Path to robot xacro or urdf file (used to build robot_description).", + declare_description_pkg = _arg( + "description_pkg", + DESCRIPTION_PKG, + "Package containing URDF/Xacro (and optionally SRDF/worlds).", + ) + declare_config_pkg = _arg( + "config_pkg", + PACKAGE, + "Package containing MoveIt/ros2_control config YAML files (config/*).", ) - declare_robot_name_arg = DeclareLaunchArgument( - name="robot_name", - default_value="iiwa7", - description="Robot name (used for TF and naming spawned robot).", + declare_robot_name = _arg( + "robot_name", + "iiwa7", + "Robot name (used for TF and naming).", + ) + declare_world = _arg( + "world", + _share_file("description_pkg", "worlds", "iiwa.wbt"), + "Path to Webots world (.wbt).", + ) + declare_controller = _arg( + "controller", + _share_file("config_pkg", "config", "iiwa_controller.yaml"), + "Controllers YAML (spawner/controller_manager).", + ) + declare_transform = _arg( + "transform", + "-0.25 0 0.79", + "Spawn translation in Webots (x y z).", + ) + declare_rotation = _arg( + "rotation", + "0 0 1 0", + "Spawn rotation axis-angle in Webots.", + ) + declare_rviz = _arg( + "rviz", + "0", + "If true|1|yes then launch RViz/MoveIt branch (instead of controllers branch).", + ) + declare_controller_timer = _arg( + "controller_timer", + "50", + "Timeout (seconds) for controller_manager spawners.", ) - declare_world_arg = DeclareLaunchArgument( - name="world", - default_value=PathJoinSubstitution( - [FindPackageShare(DESCRIPTION_PKG), "worlds", "iiwa.wbt"] - ), - description="Path to the Webots world (.wbt) to launch.", + # URDF/SRDF + declare_xacro_file = _arg( + "xacro_file", + _share_file("description_pkg", "urdf", "iiwa7.urdf.xacro"), + "Xacro used by MoveIt robot_description.", + ) + declare_srdf_file = _arg( + "srdf_file", + _share_file("config_pkg", "config", "iiwa7.srdf"), + "SRDF path.", ) - declare_controller_arg = DeclareLaunchArgument( - name="controller", - default_value=PathJoinSubstitution( - [FindPackageShare(PACKAGE), "config", "iiwa_controller.yaml"] - ), - description="Path to controllers YAML file (used by spawner and controller_manager).", + # YAML конфиги + declare_initial_positions = _arg( + "initial_positions_file", + _share_file("config_pkg", "config", "initial_positions.yaml"), + "initial_positions.yaml passed into xacro arg initial_positions_file.", + ) + declare_kinematics_yaml = _arg( + "kinematics_yaml", + _share_file("config_pkg", "config", "kinematics.yaml"), + "MoveIt kinematics.yaml", + ) + declare_joint_limits_yaml = _arg( + "joint_limits_yaml", + _share_file("config_pkg", "config", "joint_limits.yaml"), + "MoveIt joint_limits.yaml", + ) + declare_pilz_limits_yaml = _arg( + "pilz_limits_yaml", + _share_file("config_pkg", "config", "pilz_cartesian_limits.yaml"), + "Pilz cartesian limits yaml.", + ) + declare_moveit_controllers_yaml = _arg( + "moveit_controllers_yaml", + _share_file("config_pkg", "config", "moveit_controllers.yaml"), + "MoveIt controllers (trajectory_execution / simple_controller_manager).", ) - declare_transform_arg = DeclareLaunchArgument( - name="transform", - default_value="-0.25 0 0.79", - description="Translation applied when spawning the robot in the Webots world (x y z).", - ) - - declare_rotation_arg = DeclareLaunchArgument( - name="rotation", - default_value="0 0 1 0", - description="Rotation (axis-angle) applied when spawning the robot in the Webots world.", - ) - - declare_rviz_arg = DeclareLaunchArgument( - name="rviz", - default_value="0", - description="If true|1|yes then launch RViz and joint_state_publisher_gui instead of controllers.", - ) - - declare_controller_manager_arg = DeclareLaunchArgument( - name="controller_timer", - default_value="50", - description="Timeout (seconds) for controller_manager spawners (--controller-manager-timeout).", - ) runtime_setup = OpaqueFunction(function=_runtime_setup) return LaunchDescription( [ - declare_model_arg, - declare_robot_name_arg, - declare_world_arg, - declare_controller_arg, - declare_transform_arg, - declare_rotation_arg, - declare_rviz_arg, - declare_controller_manager_arg, + declare_description_pkg, + declare_config_pkg, + + declare_robot_name, + declare_world, + declare_controller, + declare_transform, + declare_rotation, + declare_rviz, + declare_controller_timer, + + declare_xacro_file, + declare_srdf_file, + declare_initial_positions, + declare_kinematics_yaml, + declare_joint_limits_yaml, + declare_pilz_limits_yaml, + declare_moveit_controllers_yaml, + runtime_setup, ] ) diff --git a/src/iiwa_bringup/launch/rviz_iiwa.launch.py b/src/iiwa_bringup/launch/rviz_iiwa.launch.py deleted file mode 100644 index 24bb95a..0000000 --- a/src/iiwa_bringup/launch/rviz_iiwa.launch.py +++ /dev/null @@ -1,82 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.events import Shutdown -from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - package_name = "iiwa_bringup" - description_package_name = "iiwa_description" - - declare_model_arg = DeclareLaunchArgument( - name="model", - default_value=PathJoinSubstitution( - [FindPackageShare(description_package_name), "urdf", "iiwa7.urdf.xacro"] - ), - description="Path to the robot URDF/Xacro file", - ) - - declare_robot_name_arg = DeclareLaunchArgument( - name="robot_name", - default_value="iiwa7", - description="Robot name fro the TF tree", - ) - - robot_description = Command( - [ - "xacro ", - LaunchConfiguration("model"), - " robot_name:=", - LaunchConfiguration("robot_name"), - ] - ) - - robot_state_publisher = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", - output="both", - parameters=[ - { - "robot_description": robot_description, - "use_sim_time": False, - } - ], - ) - - joint_state_publisher_gui = Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - name="joint_state_publisher_gui", - parameters=[{"use_sim_time": False}], - ) - - rviz_config = PathJoinSubstitution( - [FindPackageShare(package_name), "config", "rviz_iiwa.rviz"] - ) - - rviz = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - arguments=["-d", rviz_config], - output="log", - ) - - shutdown_on_rviz_exit = RegisterEventHandler( - OnProcessExit(target_action=rviz, on_exit=[EmitEvent(event=Shutdown())]) - ) - - return LaunchDescription( - [ - declare_model_arg, - declare_robot_name_arg, - robot_state_publisher, - joint_state_publisher_gui, - rviz, - shutdown_on_rviz_exit, - ] - ) diff --git a/src/iiwa_bringup/launch/supported/rviz.launch.py b/src/iiwa_bringup/launch/supported/rviz.launch.py deleted file mode 100644 index 7318deb..0000000 --- a/src/iiwa_bringup/launch/supported/rviz.launch.py +++ /dev/null @@ -1,36 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.events import Shutdown -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - declare_package_arg = DeclareLaunchArgument( - "package_name", - default_value="iiwa_bringup", - description="Package name where RViz config is stored", - ) - - package_name = LaunchConfiguration("package_name") - - rviz_config = PathJoinSubstitution( - [FindPackageShare(package_name), "config", "rviz_iiwa.rviz"] - ) - - rviz = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - arguments=["-d", rviz_config], - output="log", - ) - - # При закрытии rviz закрываем все - shutdown_on_rviz_exit = RegisterEventHandler( - OnProcessExit(target_action=rviz, on_exit=[EmitEvent(event=Shutdown())]) - ) - - return LaunchDescription([declare_package_arg, rviz, shutdown_on_rviz_exit]) diff --git a/src/iiwa_bringup/launch/supported/webots_controllers.launch.py b/src/iiwa_bringup/launch/supported/webots_controllers.launch.py index 0353b31..ffee1ab 100644 --- a/src/iiwa_bringup/launch/supported/webots_controllers.launch.py +++ b/src/iiwa_bringup/launch/supported/webots_controllers.launch.py @@ -4,21 +4,23 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from webots_ros2_driver.urdf_spawner import URDFSpawner + from iiwa_bringup.utils import converter def _setup_controllers(context, *args, **kwargs): - model_path = LaunchConfiguration("model").perform(context) + xacro_file = LaunchConfiguration("xacro_file").perform(context) robot_name = LaunchConfiguration("robot_name").perform(context) transform = LaunchConfiguration("transform").perform(context) rotation = LaunchConfiguration("rotation").perform(context) timer = LaunchConfiguration("controller_timer").perform(context) robot_description = converter.load_robot_description( - model_path=model_path, robot_name=robot_name + model_path=xacro_file, robot_name=robot_name ) tmo = ["--controller-manager-timeout", str(timer)] + jsb = Node( package="controller_manager", diff --git a/src/iiwa_bringup/launch/supported/webots_spawn.launch.py b/src/iiwa_bringup/launch/supported/webots_spawn.launch.py index 9b1d69c..23d1e59 100644 --- a/src/iiwa_bringup/launch/supported/webots_spawn.launch.py +++ b/src/iiwa_bringup/launch/supported/webots_spawn.launch.py @@ -16,7 +16,7 @@ from webots_ros2_driver.webots_launcher import WebotsLauncher def _spawn_setup(context, *args, **kwargs): - model_path = LaunchConfiguration("model").perform(context) + xacro_file = LaunchConfiguration("xacro_file").perform(context) robot_name = LaunchConfiguration("robot_name").perform(context) world_path = LaunchConfiguration("world").perform(context) transform = LaunchConfiguration("transform").perform(context) @@ -29,13 +29,13 @@ def _spawn_setup(context, *args, **kwargs): robot_name=robot_name, parameters=[ { - "robot_description": model_path, - "use_sim_time": True, + "robot_description": xacro_file, + "use_sim_time": False, "set_robot_state_publisher": False, }, LaunchConfiguration("controller").perform(context), ], - respawn=False, + respawn=True, ) controllers_launch = IncludeLaunchDescription( @@ -51,7 +51,7 @@ def _spawn_setup(context, *args, **kwargs): ), launch_arguments={ "robot_name": robot_name, - "model": model_path, + "xacro_file": xacro_file, "transform": transform, "rotation": rotation, "controller_timer": timer, diff --git a/src/iiwa_description/urdf/iiwa7.urdf b/src/iiwa_description/urdf/iiwa7.urdf deleted file mode 100644 index 3f004ba..0000000 --- a/src/iiwa_description/urdf/iiwa7.urdf +++ /dev/null @@ -1,280 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - webots_ros2_control::Ros2ControlSystem - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/iiwa_description/urdf/iiwa7.urdf.xacro b/src/iiwa_description/urdf/iiwa7.urdf.xacro index c4a4248..0915e38 100644 --- a/src/iiwa_description/urdf/iiwa7.urdf.xacro +++ b/src/iiwa_description/urdf/iiwa7.urdf.xacro @@ -1,6 +1,14 @@ - + + + + + + + @@ -13,41 +21,50 @@ webots_ros2_control::Ros2ControlSystem + - - + + + ${initial_positions['joint1']} + - - + + ${initial_positions['joint2']} + - - + + ${initial_positions['joint3']} + - - + + ${initial_positions['joint4']} + - - + + ${initial_positions['joint5']} + - - + + ${initial_positions['joint6']} + - - + + ${initial_positions['joint7']} + diff --git a/src/iiwa_description/urdf/params.xacro b/src/iiwa_description/urdf/params.xacro index 0ed46e8..5a5e7db 100644 --- a/src/iiwa_description/urdf/params.xacro +++ b/src/iiwa_description/urdf/params.xacro @@ -1,6 +1,6 @@ - +