From 66e999514f8639e0735b2138cca3d3af551bf28b Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Sep 2023 16:14:04 +0100 Subject: [PATCH 01/29] initial ignition gazebo changes --- lbr_description/gazebo/lbr.gazebo.xacro | 2 +- lbr_description/lbr_description.dsv | 2 +- lbr_description/lbr_description/launch_mixin.py | 9 +++++---- lbr_description/ros2_control/lbr.ros2_control.xacro | 2 +- 4 files changed, 8 insertions(+), 7 deletions(-) diff --git a/lbr_description/gazebo/lbr.gazebo.xacro b/lbr_description/gazebo/lbr.gazebo.xacro index d7d4e07d..da6c6899 100644 --- a/lbr_description/gazebo/lbr.gazebo.xacro +++ b/lbr_description/gazebo/lbr.gazebo.xacro @@ -4,7 +4,7 @@ - + $(find lbr_hardware_interface)/config/lbr_controllers.yml /${robot_name} diff --git a/lbr_description/lbr_description.dsv b/lbr_description/lbr_description.dsv index cd4ca905..a00fa973 100644 --- a/lbr_description/lbr_description.dsv +++ b/lbr_description/lbr_description.dsv @@ -1 +1 @@ -prepend-non-duplicate;GAZEBO_MODEL_PATH;share \ No newline at end of file +prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share \ No newline at end of file diff --git a/lbr_description/lbr_description/launch_mixin.py b/lbr_description/lbr_description/launch_mixin.py index d248890c..28b7ded3 100644 --- a/lbr_description/lbr_description/launch_mixin.py +++ b/lbr_description/lbr_description/launch_mixin.py @@ -13,15 +13,16 @@ class GazeboMixin: + # https://answers.gazebosim.org//question/28813/how-to-spawn-a-urdf-robot-into-a-ignition-gazebo-world-from-ros2/ @staticmethod def include_gazebo(**kwargs) -> IncludeLaunchDescription: return IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( [ - FindPackageShare("gazebo_ros"), + FindPackageShare("ros_gz_sim"), "launch", - "gazebo.launch.py", + "gz_sim.launch.py", ] ) ), @@ -35,8 +36,8 @@ def node_spawn_entity( if robot_name is None: robot_name = LaunchConfiguration("robot_name") return Node( - package="gazebo_ros", - executable="spawn_entity.py", + package="ros_gz_sim", + executable="create", arguments=[ "-topic", "robot_description", diff --git a/lbr_description/ros2_control/lbr.ros2_control.xacro b/lbr_description/ros2_control/lbr.ros2_control.xacro index c886646e..9e7c994d 100644 --- a/lbr_description/ros2_control/lbr.ros2_control.xacro +++ b/lbr_description/ros2_control/lbr.ros2_control.xacro @@ -17,7 +17,7 @@ - gazebo_ros2_control/GazeboSystem + ign_ros2_control/IgnitionSystem From 325447da804462e364d058193938ac6d15247e0e Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Sep 2023 16:55:24 +0100 Subject: [PATCH 02/29] added sim run arg --- lbr_description/lbr_description/launch_mixin.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/lbr_description/lbr_description/launch_mixin.py b/lbr_description/lbr_description/launch_mixin.py index 28b7ded3..11c29113 100644 --- a/lbr_description/lbr_description/launch_mixin.py +++ b/lbr_description/lbr_description/launch_mixin.py @@ -24,8 +24,9 @@ def include_gazebo(**kwargs) -> IncludeLaunchDescription: "launch", "gz_sim.launch.py", ] - ) + ), ), + launch_arguments={"gz_args": "-r"}.items(), **kwargs ) From 000f4b58a962fa7fcf36b58ce5d36037ed04e6f9 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Sep 2023 17:06:15 +0100 Subject: [PATCH 03/29] added dep --- lbr_bringup/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index fb2b3d74..b9710bfb 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -20,6 +20,7 @@ robot_state_publisher gazebo_ros gazebo_ros2_control + ign_ros_control rviz2 rclpy From 173253bf23eb4fc8cdbba8b36e13a0df0933bce7 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Sep 2023 17:06:54 +0100 Subject: [PATCH 04/29] added empty world --- lbr_description/lbr_description/launch_mixin.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lbr_description/lbr_description/launch_mixin.py b/lbr_description/lbr_description/launch_mixin.py index 11c29113..9e119cb2 100644 --- a/lbr_description/lbr_description/launch_mixin.py +++ b/lbr_description/lbr_description/launch_mixin.py @@ -26,7 +26,7 @@ def include_gazebo(**kwargs) -> IncludeLaunchDescription: ] ), ), - launch_arguments={"gz_args": "-r"}.items(), + launch_arguments={"gz_args": "-r empty.sdf"}.items(), **kwargs ) From 66feb9ace59eb544c0efcb48ca41a8957ce82e12 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Sep 2023 17:57:21 +0100 Subject: [PATCH 05/29] updated name argument --- lbr_description/lbr_description/launch_mixin.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lbr_description/lbr_description/launch_mixin.py b/lbr_description/lbr_description/launch_mixin.py index 9e119cb2..c34c0998 100644 --- a/lbr_description/lbr_description/launch_mixin.py +++ b/lbr_description/lbr_description/launch_mixin.py @@ -42,7 +42,7 @@ def node_spawn_entity( arguments=[ "-topic", "robot_description", - "-entity", + "-name", LaunchConfiguration("robot_name"), ], output="screen", From a8962b04668e2b15030bd88cf2e2e9655306332a Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Sep 2023 22:34:57 +0100 Subject: [PATCH 06/29] added classic mixin --- .../lbr_description/launch_mixin.py | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/lbr_description/lbr_description/launch_mixin.py b/lbr_description/lbr_description/launch_mixin.py index c34c0998..1c982894 100644 --- a/lbr_description/lbr_description/launch_mixin.py +++ b/lbr_description/lbr_description/launch_mixin.py @@ -12,6 +12,42 @@ from launch_ros.substitutions import FindPackageShare +class GazeboClassicMixin: + @staticmethod + def include_gazebo(**kwargs) -> IncludeLaunchDescription: + return IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("gazebo_ros"), + "launch", + "gazebo.launch.py", + ] + ) + ), + **kwargs + ) + + @staticmethod + def node_spawn_entity( + robot_name: Optional[Union[LaunchConfiguration, str]] = None, **kwargs + ) -> Node: + if robot_name is None: + robot_name = LaunchConfiguration("robot_name") + return Node( + package="gazebo_ros", + executable="spawn_entity.py", + arguments=[ + "-topic", + "robot_description", + "-entity", + LaunchConfiguration("robot_name"), + ], + output="screen", + **kwargs + ) + + class GazeboMixin: # https://answers.gazebosim.org//question/28813/how-to-spawn-a-urdf-robot-into-a-ignition-gazebo-world-from-ros2/ @staticmethod From 5f40de36886444f60e4876b4975c9ac80f45b20b Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Sep 2023 22:35:04 +0100 Subject: [PATCH 07/29] added classic mixin --- lbr_description/lbr_description/__init__.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/lbr_description/lbr_description/__init__.py b/lbr_description/lbr_description/__init__.py index e811a21e..6994a976 100644 --- a/lbr_description/lbr_description/__init__.py +++ b/lbr_description/lbr_description/__init__.py @@ -1 +1,6 @@ -from .launch_mixin import GazeboMixin, LBRDescriptionMixin, RVizMixin +from .launch_mixin import ( + GazeboClassicMixin, + GazeboMixin, + LBRDescriptionMixin, + RVizMixin, +) From 7dc29c53c91972f10106bbfb19aefec59a7cddf9 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Sep 2023 13:16:05 +0100 Subject: [PATCH 08/29] simplified urdfs --- .../ros2_control/lbr.ros2_control.xacro | 185 +++++------------- .../urdf/iiwa14/iiwa14_description.urdf.xacro | 84 ++++---- .../urdf/iiwa7/iiwa7_description.urdf.xacro | 84 ++++---- .../urdf/med14/med14_description.urdf.xacro | 84 ++++---- .../urdf/med7/med7_description.urdf.xacro | 84 ++++---- 5 files changed, 212 insertions(+), 309 deletions(-) diff --git a/lbr_description/ros2_control/lbr.ros2_control.xacro b/lbr_description/ros2_control/lbr.ros2_control.xacro index 9e7c994d..fd621e27 100644 --- a/lbr_description/ros2_control/lbr.ros2_control.xacro +++ b/lbr_description/ros2_control/lbr.ros2_control.xacro @@ -2,13 +2,13 @@ @@ -53,140 +53,43 @@ - - - - -${joint_0_position_limit} - ${joint_0_position_limit} - - - -${effort_limit} - ${effort_limit} - - - - - - - - - - - - - - -${joint_1_position_limit} - ${joint_1_position_limit} - - - -${effort_limit} - ${effort_limit} - - - - - - - - - - - - - - -${joint_2_position_limit} - ${joint_2_position_limit} - - - -${effort_limit} - ${effort_limit} - - - - - - - - - - - - - - -${joint_3_position_limit} - ${joint_3_position_limit} - - - -${effort_limit} - ${effort_limit} - - - - - - - - - - - - - - -${joint_4_position_limit} - ${joint_4_position_limit} - - - -${effort_limit} - ${effort_limit} - - - - - - - - - - - - - - -${joint_5_position_limit} - ${joint_5_position_limit} - - - -${effort_limit} - ${effort_limit} - - - - - - - - - - - - - - -${joint_6_position_limit} - ${joint_6_position_limit} - - - -${effort_limit} - ${effort_limit} - - - - - - - - - - - + + + + ${min_position} + ${max_position} + + + -${effort_limit} + ${effort_limit} + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro index cff2e900..be0d66a1 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro @@ -6,20 +6,20 @@ - - - - - - - - - - - - - - + + + + + + + + + + + + + + @@ -63,9 +63,9 @@ - + @@ -96,9 +96,9 @@ - + @@ -129,9 +129,9 @@ - + @@ -162,9 +162,9 @@ - + @@ -195,9 +195,9 @@ - + @@ -228,9 +228,9 @@ - + @@ -261,9 +261,9 @@ - + @@ -301,13 +301,13 @@ diff --git a/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro b/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro index 58d8167d..363c3cdb 100644 --- a/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro @@ -6,20 +6,20 @@ - - - - - - - - - - - - - - + + + + + + + + + + + + + + @@ -63,9 +63,9 @@ - + @@ -96,9 +96,9 @@ - + @@ -129,9 +129,9 @@ - + @@ -162,9 +162,9 @@ - + @@ -195,9 +195,9 @@ - + @@ -228,9 +228,9 @@ - + @@ -262,9 +262,9 @@ - + @@ -302,13 +302,13 @@ diff --git a/lbr_description/urdf/med14/med14_description.urdf.xacro b/lbr_description/urdf/med14/med14_description.urdf.xacro index 59ece9c7..fb10b096 100644 --- a/lbr_description/urdf/med14/med14_description.urdf.xacro +++ b/lbr_description/urdf/med14/med14_description.urdf.xacro @@ -6,20 +6,20 @@ - - - - - - - - - - - - - - + + + + + + + + + + + + + + @@ -63,9 +63,9 @@ - + @@ -96,9 +96,9 @@ - + @@ -129,9 +129,9 @@ - + @@ -162,9 +162,9 @@ - + @@ -195,9 +195,9 @@ - + @@ -228,9 +228,9 @@ - + @@ -261,9 +261,9 @@ - + @@ -301,13 +301,13 @@ diff --git a/lbr_description/urdf/med7/med7_description.urdf.xacro b/lbr_description/urdf/med7/med7_description.urdf.xacro index a751972b..bd71cc76 100644 --- a/lbr_description/urdf/med7/med7_description.urdf.xacro +++ b/lbr_description/urdf/med7/med7_description.urdf.xacro @@ -6,20 +6,20 @@ - - - - - - - - - - - - - - + + + + + + + + + + + + + + @@ -63,9 +63,9 @@ - + @@ -96,9 +96,9 @@ - + @@ -129,9 +129,9 @@ - + @@ -162,9 +162,9 @@ - + @@ -195,9 +195,9 @@ - + @@ -228,9 +228,9 @@ - + @@ -261,9 +261,9 @@ - + @@ -301,13 +301,13 @@ From 865fec708b7a05d13238c328caf6b3af07b25ede Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Sep 2023 13:17:56 +0100 Subject: [PATCH 09/29] fixed names --- .../ros2_control/lbr.ros2_control.xacro | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/lbr_description/ros2_control/lbr.ros2_control.xacro b/lbr_description/ros2_control/lbr.ros2_control.xacro index fd621e27..d1d9b84e 100644 --- a/lbr_description/ros2_control/lbr.ros2_control.xacro +++ b/lbr_description/ros2_control/lbr.ros2_control.xacro @@ -76,20 +76,20 @@ - - - - - - - + + + + + + + \ No newline at end of file From dfdf21efa148260e07b64a3b3f3bf627543ae3e0 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Sep 2023 13:22:38 +0100 Subject: [PATCH 10/29] updated ros2 control effort reject --- .../src/lbr_joint_trajectory_executioner_node.cpp | 1 - .../lbr_joint_trajectory_executioner_node.py | 1 - 2 files changed, 2 deletions(-) diff --git a/lbr_demos/lbr_demos_ros2_control_cpp/src/lbr_joint_trajectory_executioner_node.cpp b/lbr_demos/lbr_demos_ros2_control_cpp/src/lbr_joint_trajectory_executioner_node.cpp index 17648435..fe5af946 100644 --- a/lbr_demos/lbr_demos_ros2_control_cpp/src/lbr_joint_trajectory_executioner_node.cpp +++ b/lbr_demos/lbr_demos_ros2_control_cpp/src/lbr_joint_trajectory_executioner_node.cpp @@ -37,7 +37,6 @@ class LBRJointTrajectoryExecutionerNode : public rclcpp::Node { trajectory_msgs::msg::JointTrajectoryPoint point; point.positions = positions; point.velocities.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 0.0); - point.effort.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 0.0); point.time_from_start.sec = sec_from_start; for (std::size_t i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) { diff --git a/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/lbr_joint_trajectory_executioner_node.py b/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/lbr_joint_trajectory_executioner_node.py index 425c0835..2348efad 100644 --- a/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/lbr_joint_trajectory_executioner_node.py +++ b/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/lbr_joint_trajectory_executioner_node.py @@ -32,7 +32,6 @@ def execute(self, positions: list, sec_from_start: int = 10): point = JointTrajectoryPoint() point.positions = positions point.velocities = [0.0] * len(positions) - point.effort = [0.0] * len(positions) point.time_from_start.sec = sec_from_start for i in range(7): From d9a4065257c041ec61e943b6b7e940101c065820 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Sep 2023 13:47:45 +0100 Subject: [PATCH 11/29] updated tests --- .../test/lbr_model_specifications.py | 12 ----- lbr_description/test/test_urdf.py | 45 ++++++++----------- 2 files changed, 18 insertions(+), 39 deletions(-) diff --git a/lbr_description/test/lbr_model_specifications.py b/lbr_description/test/lbr_model_specifications.py index e15cdc6d..090fa100 100644 --- a/lbr_description/test/lbr_model_specifications.py +++ b/lbr_description/test/lbr_model_specifications.py @@ -36,18 +36,6 @@ def __init__( self.joint_limits = joint_limits -# reference URDF joint names to KUKA joint names -URDF_TO_KUKA_JOINT_NAME_DICT: Dict[str, str] = { - "joint_0": "A1", - "joint_1": "A2", - "joint_2": "A3", - "joint_3": "A4", - "joint_4": "A5", - "joint_5": "A6", - "joint_6": "A7", -} - - # specifications as extracted from https://xpert.kuka.com/ LBR_SPECIFICATIONS_DICT: Dict[str, Type[LBRSpecification]] = { "AR7606": LBRSpecification( diff --git a/lbr_description/test/test_urdf.py b/lbr_description/test/test_urdf.py index af6b88db..e36372c9 100644 --- a/lbr_description/test/test_urdf.py +++ b/lbr_description/test/test_urdf.py @@ -6,13 +6,9 @@ import pytest import xacro from ament_index_python import get_package_share_directory -from urdf_parser_py.urdf import URDF, Robot +from urdf_parser_py.urdf import URDF -from .lbr_model_specifications import ( - LBR_SPECIFICATIONS_DICT, - URDF_TO_KUKA_JOINT_NAME_DICT, - LBRSpecification, -) +from .lbr_model_specifications import LBR_SPECIFICATIONS_DICT, LBRSpecification @pytest.fixture @@ -71,25 +67,22 @@ def test_position_limits( for joint in urdf.joints: if joint.type == "revolute": - urdf_joint_name = "_".join(joint.name.split("_")[-2:]) - kuka_joint_name = URDF_TO_KUKA_JOINT_NAME_DICT[urdf_joint_name] - urdf_min_position = joint.limit.lower kuka_min_position = math.radians( - lbr_specification.joint_limits[kuka_joint_name].min_position + lbr_specification.joint_limits[joint.name].min_position ) if not math.isclose(urdf_min_position, kuka_min_position, abs_tol=abs_tol): raise ValueError( - f"Expected minimum joint position {kuka_min_position} rad, found {urdf_min_position} rad for model {lbr_specification.name} and joint {kuka_joint_name}/{urdf_joint_name}." + f"Expected minimum joint position {kuka_min_position} rad, found {urdf_min_position} rad for model {lbr_specification.name} and joint {joint.name}." ) urdf_max_position = joint.limit.upper kuka_max_position = math.radians( - lbr_specification.joint_limits[kuka_joint_name].max_position + lbr_specification.joint_limits[joint.name].max_position ) if not math.isclose(urdf_max_position, kuka_max_position, abs_tol=abs_tol): raise ValueError( - f"Expected maximum joint position {kuka_max_position} rad, found {urdf_max_position} rad for model {lbr_specification.name} and joint {kuka_joint_name}/{urdf_joint_name}." + f"Expected maximum joint position {kuka_max_position} rad, found {urdf_max_position} rad for model {lbr_specification.name} and joint {joint.name}." ) @@ -102,16 +95,13 @@ def test_velocity_limits( for joint in urdf.joints: if joint.type == "revolute": - urdf_joint_name = "_".join(joint.name.split("_")[-2:]) - kuka_joint_name = URDF_TO_KUKA_JOINT_NAME_DICT[urdf_joint_name] - urdf_max_velocity = joint.limit.velocity kuka_max_velcoity = math.radians( - lbr_specification.joint_limits[kuka_joint_name].max_velocity + lbr_specification.joint_limits[joint.name].max_velocity ) if not math.isclose(urdf_max_velocity, kuka_max_velcoity, abs_tol=abs_tol): raise ValueError( - f"Expected minimum joint position {kuka_max_velcoity} rad/s, found {urdf_max_velocity} rad/s for model {lbr_specification.name} and joint {kuka_joint_name}/{urdf_joint_name}." + f"Expected minimum joint position {kuka_max_velcoity} rad/s, found {urdf_max_velocity} rad/s for model {lbr_specification.name} and joint {joint.name}." ) @@ -121,34 +111,35 @@ def test_position_limits_ros2_control( ) -> None: xml, lbr_specification = setup_xml_and_reference xml = ET.ElementTree(ET.fromstring(xml)) - for joint in xml.find("ros2_control").iter("joint"): - urdf_joint_name = "_".join(joint.get("name").split("_")[-2:]) - kuka_joint_name = URDF_TO_KUKA_JOINT_NAME_DICT[urdf_joint_name] - - for command_interface in joint.iter("command_interface"): + for joint_interface in xml.find("ros2_control").iter("joint_interface"): + for command_interface in joint_interface.iter("command_interface"): if command_interface.get("name") == "position": for param in command_interface.iter("param"): if param.get("name") == "min": urdf_min_position = float(param.text) kuka_min_position = math.radians( - lbr_specification.joint_limits[kuka_joint_name].min_position + lbr_specification.joint_limits[ + joint_interface.name + ].min_position ) if not math.isclose( urdf_min_position, kuka_min_position, abs_tol=abs_tol ): raise ValueError( - f"Expected minimum joint position {kuka_min_position} rad, found {urdf_min_position} rad for model {lbr_specification.name} and position command interface joint {kuka_joint_name}/{urdf_joint_name}." + f"Expected minimum joint position {kuka_min_position} rad, found {urdf_min_position} rad for model {lbr_specification.name} and position command interface joint {joint_interface.name}." ) elif param.get("name") == "max": urdf_max_position = float(param.text) kuka_max_position = math.radians( - lbr_specification.joint_limits[kuka_joint_name].max_position + lbr_specification.joint_limits[ + joint_interface.name + ].max_position ) if not math.isclose( urdf_max_position, kuka_max_position, abs_tol=abs_tol ): raise ValueError( - f"Expected maximum joint position {kuka_max_position} rad, found {urdf_max_position} rad for model {lbr_specification.name} and position command interface joint {kuka_joint_name}/{urdf_joint_name}." + f"Expected maximum joint position {kuka_max_position} rad, found {urdf_max_position} rad for model {lbr_specification.name} and position command interface joint {joint_interface.name}." ) else: raise ValueError("Couldn't find name.") From b0dfd9b453559661ebb7ca496231eb99841b73e2 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Sep 2023 13:54:11 +0100 Subject: [PATCH 12/29] simplified gazebo xacro --- lbr_description/gazebo/lbr.gazebo.xacro | 126 +++++++----------------- 1 file changed, 34 insertions(+), 92 deletions(-) diff --git a/lbr_description/gazebo/lbr.gazebo.xacro b/lbr_description/gazebo/lbr.gazebo.xacro index da6c6899..5e4253a6 100644 --- a/lbr_description/gazebo/lbr.gazebo.xacro +++ b/lbr_description/gazebo/lbr.gazebo.xacro @@ -1,103 +1,45 @@ - + - + $(find lbr_hardware_interface)/config/lbr_controllers.yml - /${robot_name} - - - 0.2 - 0.2 - - - - - true - true - - - - - 0.2 - 0.2 - - - - - true - true - - - - - 0.2 - 0.2 - - - - - true - true - - - - - 0.2 - 0.2 - - - - - true - true - - - - - 0.2 - 0.2 - - - - - true - true - - - - - 0.2 - 0.2 - - - - - true - true - - - - - 0.2 - 0.2 - - - - - true - true - - - - - 0.2 - 0.2 - + + + + 0.2 + 0.2 + + + + + + true + true + + + + + + + + + + + + + + + + + + + \ No newline at end of file From 2756ce9908a9155eca7d0451e7cff42b0314d36c Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Sep 2023 15:17:35 +0100 Subject: [PATCH 13/29] renamed controller --- .../src/lbr_joint_trajectory_executioner_node.cpp | 2 +- .../lbr_joint_trajectory_executioner_node.py | 2 +- lbr_hardware_interface/config/lbr_controllers.yml | 4 ++-- .../lbr_hardware_interface/launch_mixin.py | 6 +++--- lbr_moveit_config/doc/lbr_moveit_config.rst | 2 +- .../iiwa14_moveit_config/config/moveit_controllers.yaml | 4 ++-- .../iiwa7_moveit_config/config/moveit_controllers.yaml | 4 ++-- .../med14_moveit_config/config/moveit_controllers.yaml | 4 ++-- .../med7_moveit_config/config/moveit_controllers.yaml | 4 ++-- 9 files changed, 16 insertions(+), 16 deletions(-) diff --git a/lbr_demos/lbr_demos_ros2_control_cpp/src/lbr_joint_trajectory_executioner_node.cpp b/lbr_demos/lbr_demos_ros2_control_cpp/src/lbr_joint_trajectory_executioner_node.cpp index fe5af946..d70af962 100644 --- a/lbr_demos/lbr_demos_ros2_control_cpp/src/lbr_joint_trajectory_executioner_node.cpp +++ b/lbr_demos/lbr_demos_ros2_control_cpp/src/lbr_joint_trajectory_executioner_node.cpp @@ -16,7 +16,7 @@ class LBRJointTrajectoryExecutionerNode : public rclcpp::Node { joint_trajectory_action_client_ = rclcpp_action::create_client( - this, "/position_trajectory_controller/follow_joint_trajectory"); + this, "/joint_trajectory_controller/follow_joint_trajectory"); while (!joint_trajectory_action_client_->wait_for_action_server(std::chrono::seconds(1))) { RCLCPP_INFO(this->get_logger(), "Waiting for action server to become available..."); diff --git a/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/lbr_joint_trajectory_executioner_node.py b/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/lbr_joint_trajectory_executioner_node.py index 2348efad..7cce8c44 100644 --- a/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/lbr_joint_trajectory_executioner_node.py +++ b/lbr_demos/lbr_demos_ros2_control_python/lbr_demos_ros2_control_python/lbr_joint_trajectory_executioner_node.py @@ -14,7 +14,7 @@ def __init__( self.joint_trajectory_action_client_ = ActionClient( node=self, action_type=FollowJointTrajectory, - action_name="/position_trajectory_controller/follow_joint_trajectory", + action_name="/joint_trajectory_controller/follow_joint_trajectory", ) while not self.joint_trajectory_action_client_.wait_for_server(1): self.get_logger().info("Waiting for action server to become available...") diff --git a/lbr_hardware_interface/config/lbr_controllers.yml b/lbr_hardware_interface/config/lbr_controllers.yml index 81a3dd85..e9d10c3f 100644 --- a/lbr_hardware_interface/config/lbr_controllers.yml +++ b/lbr_hardware_interface/config/lbr_controllers.yml @@ -5,13 +5,13 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - position_trajectory_controller: + joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController forward_position_controller: type: position_controllers/JointGroupPositionController -position_trajectory_controller: +joint_trajectory_controller: ros__parameters: joints: - A1 diff --git a/lbr_hardware_interface/lbr_hardware_interface/launch_mixin.py b/lbr_hardware_interface/lbr_hardware_interface/launch_mixin.py index 82431542..43e9cf1b 100644 --- a/lbr_hardware_interface/lbr_hardware_interface/launch_mixin.py +++ b/lbr_hardware_interface/lbr_hardware_interface/launch_mixin.py @@ -27,9 +27,9 @@ def arg_ctrl_cfg() -> DeclareLaunchArgument: def arg_ctrl() -> DeclareLaunchArgument: return DeclareLaunchArgument( name="ctrl", - default_value="position_trajectory_controller", + default_value="joint_trajectory_controller", description="Desired default controller. One of specified in ctrl_cfg.", - choices=["position_trajectory_controller", "forward_position_controller"], + choices=["joint_trajectory_controller", "forward_position_controller"], ) @staticmethod @@ -97,7 +97,7 @@ def node_controller(**kwargs) -> Node: executable="spawner", output="screen", arguments=[ - LaunchConfiguration("ctrl", default="position_trajectory_controller"), + LaunchConfiguration("ctrl", default="joint_trajectory_controller"), "--controller-manager", "/controller_manager", ], diff --git a/lbr_moveit_config/doc/lbr_moveit_config.rst b/lbr_moveit_config/doc/lbr_moveit_config.rst index 8fa4e195..c1627c10 100644 --- a/lbr_moveit_config/doc/lbr_moveit_config.rst +++ b/lbr_moveit_config/doc/lbr_moveit_config.rst @@ -90,7 +90,7 @@ This procedure applies to all LBRs: ``iiwa7``, ``iiwa14``, ``med7``, and ``med14 #. In the `move_group.launch.py `_ use the robot descriotion from ``lbr_description`` - #. In `moveit_controllers.yaml `_ change the ``arm_controller`` to ``position_trajectory_controller``, as in `lbr_controllers.yml `_ + #. In `moveit_controllers.yaml `_ change the ``arm_controller`` to ``joint_trajectory_controller``, as in `lbr_controllers.yml `_ Update MoveIt Configuration --------------------------- diff --git a/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml index 27b3961f..57d65c17 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml @@ -4,9 +4,9 @@ moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControll moveit_simple_controller_manager: controller_names: - - position_trajectory_controller + - joint_trajectory_controller - position_trajectory_controller: + joint_trajectory_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true diff --git a/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml index 27b3961f..57d65c17 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml @@ -4,9 +4,9 @@ moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControll moveit_simple_controller_manager: controller_names: - - position_trajectory_controller + - joint_trajectory_controller - position_trajectory_controller: + joint_trajectory_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true diff --git a/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml index 27b3961f..57d65c17 100644 --- a/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml @@ -4,9 +4,9 @@ moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControll moveit_simple_controller_manager: controller_names: - - position_trajectory_controller + - joint_trajectory_controller - position_trajectory_controller: + joint_trajectory_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true diff --git a/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml b/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml index 27b3961f..57d65c17 100644 --- a/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml @@ -4,9 +4,9 @@ moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControll moveit_simple_controller_manager: controller_names: - - position_trajectory_controller + - joint_trajectory_controller - position_trajectory_controller: + joint_trajectory_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true From b362ba5ec69864456e1774b50c9693dbc395489e Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Sep 2023 16:25:51 +0100 Subject: [PATCH 14/29] removed robot name --- lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro | 5 ++--- lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro | 5 ++--- lbr_description/urdf/med14/med14_description.urdf.xacro | 5 ++--- lbr_description/urdf/med7/med7_description.urdf.xacro | 5 ++--- 4 files changed, 8 insertions(+), 12 deletions(-) diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro index be0d66a1..4d43ca4c 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro @@ -295,10 +295,9 @@ - - + - + - - + - + - - + - + - - + - + Date: Tue, 2 Jan 2024 18:09:36 +0000 Subject: [PATCH 15/29] re-add position limits to system interface --- lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro | 7 +++++++ lbr_description/urdf/iiwa7/iiwa7_description.urdf.xacro | 7 +++++++ lbr_description/urdf/med14/med14_description.urdf.xacro | 7 +++++++ lbr_description/urdf/med7/med7_description.urdf.xacro | 7 +++++++ 4 files changed, 28 insertions(+) diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro index 80038e63..de597244 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro @@ -285,6 +285,13 @@ Date: Tue, 2 Jan 2024 18:30:22 +0000 Subject: [PATCH 16/29] fixed ign ros2 control dependency --- lbr_bringup/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index 26b61236..afe73646 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -20,7 +20,7 @@ robot_state_publisher gazebo_ros gazebo_ros2_control - ign_ros_control + ign_ros2_control rviz2 rclpy From fd6c74823a57ad732f21966ff7d3087fac38c8f2 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 2 Jan 2024 19:27:25 +0000 Subject: [PATCH 17/29] updated launch --- lbr_bringup/launch/sim.launch.py | 8 +++++--- lbr_description/gazebo/lbr.gazebo.xacro | 6 ++++-- lbr_description/lbr_description/__init__.py | 2 +- lbr_description/lbr_description/launch_mixin.py | 17 ++++++++++------- .../urdf/iiwa14/iiwa14_description.urdf.xacro | 2 +- .../urdf/iiwa7/iiwa7_description.urdf.xacro | 2 +- .../urdf/med14/med14_description.urdf.xacro | 2 +- .../urdf/med7/med7_description.urdf.xacro | 2 +- 8 files changed, 24 insertions(+), 17 deletions(-) diff --git a/lbr_bringup/launch/sim.launch.py b/lbr_bringup/launch/sim.launch.py index 9fd377f9..70413bba 100644 --- a/lbr_bringup/launch/sim.launch.py +++ b/lbr_bringup/launch/sim.launch.py @@ -12,7 +12,7 @@ ) from lbr_bringup import LBRMoveGroupMixin -from lbr_description import GazeboMixin, LBRDescriptionMixin, RVizMixin +from lbr_description import IgnitionGazeboMixin, LBRDescriptionMixin, RVizMixin from lbr_ros2_control import LBRROS2ControlMixin @@ -20,8 +20,10 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ld = LaunchDescription() robot_description = LBRDescriptionMixin.param_robot_description(sim=True) - ld.add_action(GazeboMixin.include_gazebo()) # Gazebo has its own controller manager - spawn_entity = GazeboMixin.node_spawn_entity() + ld.add_action( + IgnitionGazeboMixin.include_gazebo() + ) # Gazebo has its own controller manager + spawn_entity = IgnitionGazeboMixin.node_spawn_entity() ld.add_action(spawn_entity) joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( controller="joint_state_broadcaster" diff --git a/lbr_description/gazebo/lbr.gazebo.xacro b/lbr_description/gazebo/lbr.gazebo.xacro index bf196b44..a828c0d5 100644 --- a/lbr_description/gazebo/lbr.gazebo.xacro +++ b/lbr_description/gazebo/lbr.gazebo.xacro @@ -1,13 +1,15 @@ - + $(find lbr_ros2_control)/config/lbr_controllers.yaml - /${robot_name} + + /${robot_name} + diff --git a/lbr_description/lbr_description/__init__.py b/lbr_description/lbr_description/__init__.py index 6994a976..8231d847 100644 --- a/lbr_description/lbr_description/__init__.py +++ b/lbr_description/lbr_description/__init__.py @@ -1,6 +1,6 @@ from .launch_mixin import ( - GazeboClassicMixin, GazeboMixin, + IgnitionGazeboMixin, LBRDescriptionMixin, RVizMixin, ) diff --git a/lbr_description/lbr_description/launch_mixin.py b/lbr_description/lbr_description/launch_mixin.py index 2b508a58..c7bc7aa6 100644 --- a/lbr_description/lbr_description/launch_mixin.py +++ b/lbr_description/lbr_description/launch_mixin.py @@ -12,7 +12,7 @@ from launch_ros.substitutions import FindPackageShare -class GazeboClassicMixin: +class GazeboMixin: @staticmethod def include_gazebo(**kwargs) -> IncludeLaunchDescription: return IncludeLaunchDescription( @@ -42,8 +42,6 @@ def node_spawn_entity( "robot_description", "-entity", LaunchConfiguration("robot_name"), - "-robot_namespace", - LaunchConfiguration("robot_name"), ], output="screen", namespace=LaunchConfiguration("robot_name"), @@ -51,7 +49,7 @@ def node_spawn_entity( ) -class GazeboMixin: +class IgnitionGazeboMixin: # https://answers.gazebosim.org//question/28813/how-to-spawn-a-urdf-robot-into-a-ignition-gazebo-world-from-ros2/ @staticmethod def include_gazebo(**kwargs) -> IncludeLaunchDescription: @@ -66,7 +64,7 @@ def include_gazebo(**kwargs) -> IncludeLaunchDescription: ), ), launch_arguments={"gz_args": "-r empty.sdf"}.items(), - **kwargs + **kwargs, ) @staticmethod @@ -80,12 +78,17 @@ def node_spawn_entity( executable="create", arguments=[ "-topic", - "robot_description", + PathJoinSubstitution( + [ + robot_name, + "robot_description", + ] + ), "-name", LaunchConfiguration("robot_name"), ], output="screen", - **kwargs + **kwargs, ) diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro index de597244..506f911f 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.urdf.xacro @@ -282,7 +282,7 @@ - + - + - + - + Date: Tue, 2 Jan 2024 19:40:56 +0000 Subject: [PATCH 18/29] added unless sim condition for parameters --- lbr_ros2_control/config/lbr_system_interface.xacro | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index 3f389121..77a5eb5f 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -110,10 +110,12 @@ - ${min_position} - ${max_position} - ${max_velocity} - ${max_torque} + + ${min_position} + ${max_position} + ${max_velocity} + ${max_torque} + ${min_position} ${max_position} From 4850fbd48a3e0d687489ccf2784fb1d3b2a71056 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 3 Jan 2024 12:01:34 +0000 Subject: [PATCH 19/29] fixed ign gazebo --- lbr_bringup/package.xml | 15 +++++++-------- lbr_description/package.xml | 4 ++-- .../config/lbr_system_interface.xacro | 15 ++++++++++----- 3 files changed, 19 insertions(+), 15 deletions(-) diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index afe73646..59c8cca3 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -10,19 +10,18 @@ ament_cmake ament_cmake_python - lbr_description - lbr_fri_ros2 - lbr_ros2_control controller_manager + ign_ros2_control joint_state_broadcaster joint_trajectory_controller - xacro + lbr_description + lbr_fri_ros2 + lbr_ros2_control + rclpy robot_state_publisher - gazebo_ros - gazebo_ros2_control - ign_ros2_control + ros_ign_gazebo rviz2 - rclpy + xacro ament_cmake diff --git a/lbr_description/package.xml b/lbr_description/package.xml index 62fd5434..84b2d76f 100644 --- a/lbr_description/package.xml +++ b/lbr_description/package.xml @@ -11,11 +11,11 @@ ament_cmake_pytest ament_cmake_python - gazebo_ros2_control + ign_ros2_control joint_state_publisher_gui - tf2_ros robot_state_publisher rviz2 + tf2_ros xacro python3-pytest diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index 77a5eb5f..c370e6ba 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -44,7 +44,7 @@ - gazebo_ros2_control/GazeboSystem + ign_ros2_control/IgnitionSystem @@ -110,6 +110,7 @@ + ${min_position} ${max_position} @@ -120,10 +121,14 @@ ${min_position} ${max_position} - - -${max_torque} - ${max_torque} - + + + + -${max_torque} + ${max_torque} + + From ce194c81e391819dfc1fe69422f369acbd1a661e Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 3 Jan 2024 12:16:20 +0000 Subject: [PATCH 20/29] simplified namespace --- lbr_description/lbr_description/launch_mixin.py | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/lbr_description/lbr_description/launch_mixin.py b/lbr_description/lbr_description/launch_mixin.py index c7bc7aa6..6bf822a0 100644 --- a/lbr_description/lbr_description/launch_mixin.py +++ b/lbr_description/lbr_description/launch_mixin.py @@ -78,15 +78,11 @@ def node_spawn_entity( executable="create", arguments=[ "-topic", - PathJoinSubstitution( - [ - robot_name, - "robot_description", - ] - ), + "robot_description", "-name", LaunchConfiguration("robot_name"), ], + namespace=robot_name, output="screen", **kwargs, ) From c6e94a2db119b23c6265ae680ff45dbc32a77f1c Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 3 Jan 2024 12:23:32 +0000 Subject: [PATCH 21/29] use ign_ packages for humble --- lbr_description/lbr_description/launch_mixin.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/lbr_description/lbr_description/launch_mixin.py b/lbr_description/lbr_description/launch_mixin.py index 6bf822a0..1c75df89 100644 --- a/lbr_description/lbr_description/launch_mixin.py +++ b/lbr_description/lbr_description/launch_mixin.py @@ -57,13 +57,13 @@ def include_gazebo(**kwargs) -> IncludeLaunchDescription: PythonLaunchDescriptionSource( PathJoinSubstitution( [ - FindPackageShare("ros_gz_sim"), + FindPackageShare("ros_ign_gazebo"), "launch", - "gz_sim.launch.py", + "ign_gazebo.launch.py", ] ), ), - launch_arguments={"gz_args": "-r empty.sdf"}.items(), + launch_arguments={"ign_args": "-r empty.sdf"}.items(), **kwargs, ) @@ -74,7 +74,7 @@ def node_spawn_entity( if robot_name is None: robot_name = LaunchConfiguration("robot_name") return Node( - package="ros_gz_sim", + package="ros_ign_gazebo", executable="create", arguments=[ "-topic", From 13809ea0a549e2df949e539e05bd3b5fb505abf9 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 3 Jan 2024 13:24:10 +0000 Subject: [PATCH 22/29] use gz_sim instead of ign_gazebo --- lbr_bringup/launch/real.launch.py | 2 +- lbr_bringup/launch/sim.launch.py | 6 +++--- lbr_bringup/package.xml | 2 +- lbr_description/lbr_description/launch_mixin.py | 11 ++++++----- 4 files changed, 11 insertions(+), 10 deletions(-) diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index 5bb5a279..1c564065 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -25,7 +25,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ) ld.add_action(ros2_control_node) - # joint state broad caster and controller on ros2 control node start + # joint state broadcaster and controller on ros2 control node start joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( controller="joint_state_broadcaster" ) diff --git a/lbr_bringup/launch/sim.launch.py b/lbr_bringup/launch/sim.launch.py index 70413bba..81c501fb 100644 --- a/lbr_bringup/launch/sim.launch.py +++ b/lbr_bringup/launch/sim.launch.py @@ -23,8 +23,8 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ld.add_action( IgnitionGazeboMixin.include_gazebo() ) # Gazebo has its own controller manager - spawn_entity = IgnitionGazeboMixin.node_spawn_entity() - ld.add_action(spawn_entity) + create = IgnitionGazeboMixin.node_create(robot_description=robot_description) + ld.add_action(create) joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( controller="joint_state_broadcaster" ) @@ -120,7 +120,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: # RViz event handler rviz_event_handler = RegisterEventHandler( - OnProcessExit(target_action=spawn_entity, on_exit=[rviz_moveit, rviz]) + OnProcessExit(target_action=create, on_exit=[rviz_moveit, rviz]) ) ld.add_action(rviz_event_handler) diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index 59c8cca3..3c016427 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -19,7 +19,7 @@ lbr_ros2_control rclpy robot_state_publisher - ros_ign_gazebo + ros_gz_sim rviz2 xacro diff --git a/lbr_description/lbr_description/launch_mixin.py b/lbr_description/lbr_description/launch_mixin.py index 1c75df89..bed03a65 100644 --- a/lbr_description/lbr_description/launch_mixin.py +++ b/lbr_description/lbr_description/launch_mixin.py @@ -57,9 +57,9 @@ def include_gazebo(**kwargs) -> IncludeLaunchDescription: PythonLaunchDescriptionSource( PathJoinSubstitution( [ - FindPackageShare("ros_ign_gazebo"), + FindPackageShare("ros_gz_sim"), "launch", - "ign_gazebo.launch.py", + "gz_sim.launch.py", ] ), ), @@ -68,13 +68,14 @@ def include_gazebo(**kwargs) -> IncludeLaunchDescription: ) @staticmethod - def node_spawn_entity( - robot_name: Optional[Union[LaunchConfiguration, str]] = None, **kwargs + def node_create( + robot_name: Optional[Union[LaunchConfiguration, str]] = None, + **kwargs, ) -> Node: if robot_name is None: robot_name = LaunchConfiguration("robot_name") return Node( - package="ros_ign_gazebo", + package="ros_gz_sim", executable="create", arguments=[ "-topic", From 1322d3bf0eb49e53379ec168fe6e38334380e475 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 3 Jan 2024 13:25:00 +0000 Subject: [PATCH 23/29] remvoed robot description parameter --- lbr_bringup/launch/sim.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lbr_bringup/launch/sim.launch.py b/lbr_bringup/launch/sim.launch.py index 81c501fb..fd402b24 100644 --- a/lbr_bringup/launch/sim.launch.py +++ b/lbr_bringup/launch/sim.launch.py @@ -23,7 +23,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ld.add_action( IgnitionGazeboMixin.include_gazebo() ) # Gazebo has its own controller manager - create = IgnitionGazeboMixin.node_create(robot_description=robot_description) + create = IgnitionGazeboMixin.node_create() ld.add_action(create) joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( controller="joint_state_broadcaster" From 910343df78b66b88a1f9aa8a12eb8e5c3fb7e791 Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 3 Jan 2024 15:02:38 +0000 Subject: [PATCH 24/29] controller manager: read robot description from state pub --- lbr_bringup/launch/real.launch.py | 25 ++++++++----------- .../lbr_ros2_control/launch_mixin.py | 8 ++++-- 2 files changed, 16 insertions(+), 17 deletions(-) diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index 1c564065..c0d88c20 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -3,7 +3,7 @@ from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit, OnProcessStart +from launch.event_handlers import OnProcessStart from launch.substitutions import ( AndSubstitution, LaunchConfiguration, @@ -20,9 +20,15 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ld = LaunchDescription() robot_description = LBRDescriptionMixin.param_robot_description(sim=False) - ros2_control_node = LBRROS2ControlMixin.node_ros2_control( - robot_description=robot_description + + # robot state publisher + robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher( + robot_description=robot_description, use_sim_time=False ) + ld.add_action(robot_state_publisher) + + # ros2 control node + ros2_control_node = LBRROS2ControlMixin.node_ros2_control() ld.add_action(ros2_control_node) # joint state broadcaster and controller on ros2 control node start @@ -52,17 +58,6 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ) ld.add_action(controller_event_handler) - # robot state publisher on joint state broadcaster spawn exit - robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher( - robot_description=robot_description, use_sim_time=False - ) - robot_state_publisher_event_handler = RegisterEventHandler( - OnProcessExit( - target_action=joint_state_broadcaster, on_exit=[robot_state_publisher] - ) - ) - ld.add_action(robot_state_publisher_event_handler) - # MoveIt 2 ld.add_action(LBRMoveGroupMixin.arg_allow_trajectory_execution()) ld.add_action(LBRMoveGroupMixin.arg_capabilities()) @@ -142,7 +137,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: # RViz event handler rviz_event_handler = RegisterEventHandler( OnProcessStart( - target_action=robot_state_publisher, on_start=[rviz_moveit, rviz] + target_action=joint_state_broadcaster, on_start=[rviz_moveit, rviz] ) ) ld.add_action(rviz_event_handler) diff --git a/lbr_ros2_control/lbr_ros2_control/launch_mixin.py b/lbr_ros2_control/lbr_ros2_control/launch_mixin.py index c7fce4a3..14cc2f83 100644 --- a/lbr_ros2_control/lbr_ros2_control/launch_mixin.py +++ b/lbr_ros2_control/lbr_ros2_control/launch_mixin.py @@ -47,7 +47,6 @@ def arg_use_sim_time() -> DeclareLaunchArgument: @staticmethod def node_ros2_control( - robot_description: Dict[str, str], robot_name: Optional[Union[LaunchConfiguration, str]] = None, **kwargs, ) -> Node: @@ -70,7 +69,12 @@ def node_ros2_control( ), ] ), - robot_description, + ], + remappings=[ + ( + "~/robot_description", + "robot_description", + ), ], namespace=robot_name, **kwargs, From 0710ce65269377eb853dab45fb12dcc8c3d6ac3f Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 3 Jan 2024 17:36:03 +0000 Subject: [PATCH 25/29] added a remapping --- lbr_description/gazebo/lbr.gazebo.xacro | 1 + 1 file changed, 1 insertion(+) diff --git a/lbr_description/gazebo/lbr.gazebo.xacro b/lbr_description/gazebo/lbr.gazebo.xacro index a828c0d5..11e29003 100644 --- a/lbr_description/gazebo/lbr.gazebo.xacro +++ b/lbr_description/gazebo/lbr.gazebo.xacro @@ -9,6 +9,7 @@ $(find lbr_ros2_control)/config/lbr_controllers.yaml /${robot_name} + ~/robot_description:=robot_description From 6eb8a41771ffa0f53f56c98f4c6ae1d4b25905af Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 3 Jan 2024 18:33:04 +0000 Subject: [PATCH 26/29] updated re-mappings --- lbr_bringup/launch/real.launch.py | 3 +++ lbr_bringup/launch/sim.launch.py | 3 +++ 2 files changed, 6 insertions(+) diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index c0d88c20..1de6c658 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -117,6 +117,9 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ("display_planned_path", robot_name + "/display_planned_path"), ("joint_states", robot_name + "/joint_states"), ("monitored_planning_scene", robot_name + "/monitored_planning_scene"), + ("planning_scene", robot_name + "/planning_scene"), + ("planning_scene_world", robot_name + "/planning_scene_world"), + ("recognized_object_array", robot_name + "/recognized_object_array"), ("robot_description", robot_name + "/robot_description"), ("robot_description_semantic", robot_name + "/robot_description_semantic"), ], diff --git a/lbr_bringup/launch/sim.launch.py b/lbr_bringup/launch/sim.launch.py index fd402b24..fd3c7242 100644 --- a/lbr_bringup/launch/sim.launch.py +++ b/lbr_bringup/launch/sim.launch.py @@ -101,6 +101,9 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ("display_planned_path", robot_name + "/display_planned_path"), ("joint_states", robot_name + "/joint_states"), ("monitored_planning_scene", robot_name + "/monitored_planning_scene"), + ("planning_scene", robot_name + "/planning_scene"), + ("planning_scene_world", robot_name + "/planning_scene_world"), + ("recognized_object_array", robot_name + "/recognized_object_array"), ("robot_description", robot_name + "/robot_description"), ("robot_description_semantic", robot_name + "/robot_description_semantic"), ], From 833ed94a5b773035864ba28479bad179a7867278 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 27 Sep 2024 14:20:54 +0200 Subject: [PATCH 27/29] added time bridge --- lbr_bringup/launch/gazebo.launch.py | 6 ++--- lbr_bringup/launch/move_group.launch.py | 4 +++ lbr_bringup/lbr_bringup/gazebo.py | 26 ++++++++++++++----- lbr_bringup/package.xml | 1 + lbr_description/gazebo/lbr_gazebo.xacro | 2 -- lbr_description/lbr_description.dsv | 2 +- .../config/lbr_system_interface.xacro | 2 +- 7 files changed, 29 insertions(+), 14 deletions(-) diff --git a/lbr_bringup/launch/gazebo.launch.py b/lbr_bringup/launch/gazebo.launch.py index 0ff93e5b..bc1ff586 100644 --- a/lbr_bringup/launch/gazebo.launch.py +++ b/lbr_bringup/launch/gazebo.launch.py @@ -38,10 +38,10 @@ def generate_launch_description() -> LaunchDescription: # Gazebo ld.add_action(GazeboMixin.include_gazebo()) # Gazebo has its own controller manager - spawn_entity = GazeboMixin.node_spawn_entity( - tf=world_robot_tf + ld.add_action(GazeboMixin.node_clock_bridge()) + ld.add_action( + GazeboMixin.node_create(tf=world_robot_tf) ) # spawns robot in Gazebo through robot_description topic of robot_state_publisher - ld.add_action(spawn_entity) # controllers joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( diff --git a/lbr_bringup/launch/move_group.launch.py b/lbr_bringup/launch/move_group.launch.py index 3d1f0bdd..8a966915 100644 --- a/lbr_bringup/launch/move_group.launch.py +++ b/lbr_bringup/launch/move_group.launch.py @@ -75,6 +75,10 @@ def hidden_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: "robot_description_semantic", PathJoinSubstitution([robot_name, "robot_description_semantic"]), ), + ( + "recognized_object_array", + PathJoinSubstitution([robot_name, "recognized_object_array"]), + ), ], condition=IfCondition(LaunchConfiguration("rviz")), ) diff --git a/lbr_bringup/lbr_bringup/gazebo.py b/lbr_bringup/lbr_bringup/gazebo.py index 145e052d..07e74c6c 100644 --- a/lbr_bringup/lbr_bringup/gazebo.py +++ b/lbr_bringup/lbr_bringup/gazebo.py @@ -14,17 +14,18 @@ def include_gazebo(**kwargs) -> IncludeLaunchDescription: PythonLaunchDescriptionSource( PathJoinSubstitution( [ - FindPackageShare("gazebo_ros"), + FindPackageShare("ros_gz_sim"), "launch", - "gazebo.launch.py", + "gz_sim.launch.py", ] - ) + ), ), + launch_arguments={"gz_args": "-r empty.sdf"}.items(), **kwargs, ) @staticmethod - def node_spawn_entity( + def node_create( robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( "robot_name", default="lbr" ), @@ -34,16 +35,27 @@ def node_spawn_entity( label = ["-x", "-y", "-z", "-R", "-P", "-Y"] tf = [str(x) for x in tf] return Node( - package="gazebo_ros", - executable="spawn_entity.py", + package="ros_gz_sim", + executable="create", arguments=[ "-topic", "robot_description", - "-entity", + "-name", robot_name, + "-allow_renaming", ] + [item for pair in zip(label, tf) for item in pair], output="screen", namespace=robot_name, **kwargs, ) + + @staticmethod + def node_clock_bridge(**kwargs) -> Node: + return Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], + output="screen", + **kwargs, + ) diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index eb6bc33e..b89b0ea8 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -20,6 +20,7 @@ rclpy robot_state_publisher ros_gz_sim + ros_gz_bridge rviz2 xacro diff --git a/lbr_description/gazebo/lbr_gazebo.xacro b/lbr_description/gazebo/lbr_gazebo.xacro index 12270f9e..a828c0d5 100644 --- a/lbr_description/gazebo/lbr_gazebo.xacro +++ b/lbr_description/gazebo/lbr_gazebo.xacro @@ -9,8 +9,6 @@ $(find lbr_ros2_control)/config/lbr_controllers.yaml /${robot_name} - - ~/robot_description:=robot_description diff --git a/lbr_description/lbr_description.dsv b/lbr_description/lbr_description.dsv index a00fa973..84de9292 100644 --- a/lbr_description/lbr_description.dsv +++ b/lbr_description/lbr_description.dsv @@ -1 +1 @@ -prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;share \ No newline at end of file +prepend-non-duplicate;GZ_SIM_RESOURCE_PATH;share \ No newline at end of file diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index 051eda61..23738b9a 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -100,7 +100,7 @@ - + -${max_torque} ${max_torque} From 13ef9e5dbbc74fcfc0ed4b6487177df679217ec6 Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 27 Sep 2024 14:29:27 +0200 Subject: [PATCH 28/29] bump version: 2.1.0 -> 2.1.1 --- CITATION.cff | 2 +- lbr_bringup/package.xml | 2 +- lbr_demos/lbr_demos_advanced_cpp/package.xml | 2 +- lbr_demos/lbr_demos_advanced_py/package.xml | 2 +- lbr_demos/lbr_demos_advanced_py/setup.py | 2 +- lbr_demos/lbr_demos_cpp/package.xml | 2 +- lbr_demos/lbr_demos_py/package.xml | 2 +- lbr_demos/lbr_demos_py/setup.py | 2 +- lbr_demos/lbr_moveit_cpp/package.xml | 2 +- lbr_description/package.xml | 2 +- lbr_fri_ros2/package.xml | 2 +- lbr_fri_ros2_stack/package.xml | 2 +- lbr_ros2_control/package.xml | 2 +- 13 files changed, 13 insertions(+), 13 deletions(-) diff --git a/CITATION.cff b/CITATION.cff index 90ea1234..f3772753 100644 --- a/CITATION.cff +++ b/CITATION.cff @@ -19,6 +19,6 @@ authors: title: "LBR-Stack: ROS 2 and Python Integration of KUKA FRI for Med and IIWA Robots" -version: 2.1.0 +version: 2.1.1 doi: 10.48550/arXiv.2311.12709 date-released: 2024-09-10 diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index b89b0ea8..dba3730d 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -2,7 +2,7 @@ lbr_bringup - 2.1.0 + 2.1.1 LBR launch files. mhubii Apache License 2.0 diff --git a/lbr_demos/lbr_demos_advanced_cpp/package.xml b/lbr_demos/lbr_demos_advanced_cpp/package.xml index 81e0b2e5..069d424a 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/package.xml +++ b/lbr_demos/lbr_demos_advanced_cpp/package.xml @@ -2,7 +2,7 @@ lbr_demos_advanced_cpp - 2.1.0 + 2.1.1 Advanced C++ demos for the lbr_ros2_control. mhubii Apache License 2.0 diff --git a/lbr_demos/lbr_demos_advanced_py/package.xml b/lbr_demos/lbr_demos_advanced_py/package.xml index ade1df84..3a646a59 100644 --- a/lbr_demos/lbr_demos_advanced_py/package.xml +++ b/lbr_demos/lbr_demos_advanced_py/package.xml @@ -2,7 +2,7 @@ lbr_demos_advanced_py - 2.1.0 + 2.1.1 Advanced Python demos for the lbr_ros2_control. mhubii cmower diff --git a/lbr_demos/lbr_demos_advanced_py/setup.py b/lbr_demos/lbr_demos_advanced_py/setup.py index d3dcdd20..a3ab1449 100644 --- a/lbr_demos/lbr_demos_advanced_py/setup.py +++ b/lbr_demos/lbr_demos_advanced_py/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="2.1.0", + version="2.1.1", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_demos_cpp/package.xml b/lbr_demos/lbr_demos_cpp/package.xml index f3d80e96..7172ae22 100644 --- a/lbr_demos/lbr_demos_cpp/package.xml +++ b/lbr_demos/lbr_demos_cpp/package.xml @@ -2,7 +2,7 @@ lbr_demos_cpp - 2.1.0 + 2.1.1 C++ demos for lbr_ros2_control. mhubii Apache License 2.0 diff --git a/lbr_demos/lbr_demos_py/package.xml b/lbr_demos/lbr_demos_py/package.xml index 6829f32e..dadc747e 100644 --- a/lbr_demos/lbr_demos_py/package.xml +++ b/lbr_demos/lbr_demos_py/package.xml @@ -2,7 +2,7 @@ lbr_demos_py - 2.1.0 + 2.1.1 Python demos for lbr_ros2_control. mhubii Apache License 2.0 diff --git a/lbr_demos/lbr_demos_py/setup.py b/lbr_demos/lbr_demos_py/setup.py index c3af4e8a..a38dae9e 100644 --- a/lbr_demos/lbr_demos_py/setup.py +++ b/lbr_demos/lbr_demos_py/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version="2.1.0", + version="2.1.1", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_moveit_cpp/package.xml b/lbr_demos/lbr_moveit_cpp/package.xml index dbda4e53..17bcf85c 100644 --- a/lbr_demos/lbr_moveit_cpp/package.xml +++ b/lbr_demos/lbr_moveit_cpp/package.xml @@ -2,7 +2,7 @@ lbr_moveit_cpp - 2.1.0 + 2.1.1 Demo for using MoveIt C++ API. mhubii Apache-2.0 diff --git a/lbr_description/package.xml b/lbr_description/package.xml index 794d33b5..a00fd055 100644 --- a/lbr_description/package.xml +++ b/lbr_description/package.xml @@ -2,7 +2,7 @@ lbr_description - 2.1.0 + 2.1.1 KUKA LBR description files mhubii Apache License 2.0 diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index d5d2cc6c..58e5b7ce 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -2,7 +2,7 @@ lbr_fri_ros2 - 2.1.0 + 2.1.1 The lbr_fri_ros2 package provides the Fast Robot Interface (FRI) integration into ROS 2. Robot states can be extracted and commanded. mhubii diff --git a/lbr_fri_ros2_stack/package.xml b/lbr_fri_ros2_stack/package.xml index 8025516c..0f611574 100644 --- a/lbr_fri_ros2_stack/package.xml +++ b/lbr_fri_ros2_stack/package.xml @@ -2,7 +2,7 @@ lbr_fri_ros2_stack - 2.1.0 + 2.1.1 ROS 2 stack for KUKA LBRs. mhubii Apache License 2.0 diff --git a/lbr_ros2_control/package.xml b/lbr_ros2_control/package.xml index 08e32b2f..b1326dc5 100644 --- a/lbr_ros2_control/package.xml +++ b/lbr_ros2_control/package.xml @@ -2,7 +2,7 @@ lbr_ros2_control - 2.1.0 + 2.1.1 ROS 2 hardware hardware_interface for KUKA LBR through Fast Robot Interface (FRI). mhubii Apache License 2.0 From febbf5cc8537f4b2261f7f6983f753a332f9f0ef Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 27 Sep 2024 16:01:42 +0200 Subject: [PATCH 29/29] 2.1.1 notes --- CHANGELOG.rst | 7 +++++++ CITATION.cff | 2 +- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 5ef0323a..8de17f0b 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,13 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package LBR FRI ROS 2 Stack ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Humble v2.1.1 (2024-09-27) +-------------------------- +* Adds support for the new Gazebo and removes support for Gazebo Classic (End-of-Life January 2025, refer https://community.gazebosim.org/t/gazebo-classic-end-of-life/2563). + * ``lbr_bringup``: Updated launch files and dependencies. + * ``lbr_description``: Updated ```` tag to include Gazebo plugin (see https://github.com/ros-controls/gz_ros2_control/tree/humble). + * ``lbr_ros2_control``: Changed ``gazebo_ros2_control/GazeboSystem`` -> ``ign_ros2_control/IgnitionSystem``` + Humble v2.1.0 (2024-09-10) -------------------------- * De-couple launch files from ``lbr_bringup`` for easier customization (breaking change): diff --git a/CITATION.cff b/CITATION.cff index f3772753..83e295ed 100644 --- a/CITATION.cff +++ b/CITATION.cff @@ -21,4 +21,4 @@ authors: title: "LBR-Stack: ROS 2 and Python Integration of KUKA FRI for Med and IIWA Robots" version: 2.1.1 doi: 10.48550/arXiv.2311.12709 -date-released: 2024-09-10 +date-released: 2024-09-27