diff --git a/CHANGELOG.rst b/CHANGELOG.rst index f3cc4e47..b14ac3e3 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,26 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package LBR FRI ROS 2 Stack ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Humble v2.2.0 (2024-11-20) +-------------------------- +This release backports new ``rolling`` features to ``humble``. Following has changed: + + * Related PRs: + + * https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/213 and https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/214 + + * Joints and links are now prefixed with ``lbr_`` (i.e. the robot name) + * Robot state publisher has no ``lbr/`` prefix anymore + * async + deactivateable FT estimation + * Issue with setting real-time priority fixed + * Modifiable source for ``lbr_system_config.yaml`` in launch files + + * https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/220 + + * PID on joint position commands replaced by simpler exponential filter (please test robot in T1 mode as this will affect your control) + * Introduction of twist and admittance controllers + * Configurations from ``lbr_ros2_control`` now in ``lbr_description`` (for stand alone URDF use) + Humble v2.1.2 (2024-10-18) -------------------------- * Adds MoveIt Servo demo, related to https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/50 and https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/211 diff --git a/CITATION.cff b/CITATION.cff index 203549cd..35882061 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.2 -doi: 10.48550/arXiv.2311.12709 -date-released: 2024-10-18 +version: 2.2.0 +doi: 10.21105/joss.06138 +date-released: 2024-11-20 diff --git a/README.md b/README.md index 612c46f1..8a04632e 100644 --- a/README.md +++ b/README.md @@ -78,8 +78,8 @@ Full documentation available on [Read the Docs](https://lbr-stack.readthedocs.io ```shell source install/setup.bash ros2 launch lbr_bringup rviz.launch.py \ - rviz_config_pkg:=lbr_bringup \ - rviz_config:=config/mock.rviz + rviz_cfg_pkg:=lbr_bringup \ + rviz_cfg:=config/mock.rviz ``` Now, run the [demos](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_demos/doc/lbr_demos.html). To get started with the real robot, checkout the [Hardware Setup](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_fri_ros2_stack/doc/hardware_setup.html). @@ -88,13 +88,17 @@ Now, run the [demos](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_sta If you enjoyed using this repository for your work, we would really appreciate ❤️ if you could leave a ⭐ and / or cite it, as it helps us to continue offering support. ``` -@misc{huber2023lbrstack, - title={LBR-Stack: ROS 2 and Python Integration of KUKA FRI for Med and IIWA Robots}, - author={Martin Huber and Christopher E. Mower and Sebastien Ourselin and Tom Vercauteren and Christos Bergeles}, - year={2023}, - eprint={2311.12709}, - archivePrefix={arXiv}, - primaryClass={cs.RO} +@article{Huber2024, + doi = {10.21105/joss.06138}, + url = {https://doi.org/10.21105/joss.06138}, + year = {2024}, + publisher = {The Open Journal}, + volume = {9}, + number = {103}, + pages = {6138}, + author = {Martin Huber and Christopher E. Mower and Sebastien Ourselin and Tom Vercauteren and Christos Bergeles}, + title = {LBR-Stack: ROS 2 and Python Integration of KUKA FRI for Med and IIWA Robots}, + journal = {Journal of Open Source Software} } ``` diff --git a/lbr_bringup/config/gazebo.rviz b/lbr_bringup/config/gazebo.rviz index f4486f40..cbcb143b 100644 --- a/lbr_bringup/config/gazebo.rviz +++ b/lbr_bringup/config/gazebo.rviz @@ -67,51 +67,51 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_floating_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - link_1: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false - world: + Value: true + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false @@ -119,7 +119,7 @@ Visualization Manager: Inertia: false Mass: false Name: RobotModel - TF Prefix: lbr + TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true diff --git a/lbr_bringup/config/hardware.rviz b/lbr_bringup/config/hardware.rviz index b3a3df75..597820ee 100644 --- a/lbr_bringup/config/hardware.rviz +++ b/lbr_bringup/config/hardware.rviz @@ -68,51 +68,51 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_floating_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - link_1: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false - world: + Value: true + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false @@ -120,7 +120,7 @@ Visualization Manager: Inertia: false Mass: false Name: RobotModel - TF Prefix: lbr + TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true diff --git a/lbr_bringup/config/mock.rviz b/lbr_bringup/config/mock.rviz index f4486f40..cbcb143b 100644 --- a/lbr_bringup/config/mock.rviz +++ b/lbr_bringup/config/mock.rviz @@ -67,51 +67,51 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_floating_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - link_1: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false - world: + Value: true + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false @@ -119,7 +119,7 @@ Visualization Manager: Inertia: false Mass: false Name: RobotModel - TF Prefix: lbr + TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index da2c79d5..779dc199 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -114,8 +114,8 @@ This launch file will spin up ``RViz`` for visualization. It will (see `rviz.lau .. code:: bash ros2 launch lbr_bringup rviz.launch.py \ - rviz_config_pkg:=lbr_bringup \ - rviz_config:=config/mock.rviz # [gazebo.rviz, hardware.rviz, mock.rviz] + rviz_cfg_pkg:=lbr_bringup \ + rviz_cfg:=config/mock.rviz # [gazebo.rviz, hardware.rviz, mock.rviz] .. note:: List all arguments for the launch file via ``ros2 launch lbr_bringup rviz.launch.py -s``. @@ -156,8 +156,8 @@ The below shows an example of the `rviz.launch.py LaunchDescription: ld.add_action(LBRDescriptionMixin.arg_robot_name()) ld.add_action( LBRROS2ControlMixin.arg_ctrl() - ) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_ros2_control/config/lbr_controllers.yaml + ) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_description/ros2_control/lbr_controllers.yaml - # static transform world -> robot_name/world + # static transform world -> _floating_link world_robot_tf = [0, 0, 0, 0, 0, 0] # keep zero ld.add_action( LBRDescriptionMixin.node_static_tf( tf=world_robot_tf, parent="world", - child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]), + child=PythonExpression( + ["'", LaunchConfiguration("robot_name"), "' + '_floating_link'"] + ), ) ) diff --git a/lbr_bringup/launch/hardware.launch.py b/lbr_bringup/launch/hardware.launch.py index 5240557f..cffdf59b 100644 --- a/lbr_bringup/launch/hardware.launch.py +++ b/lbr_bringup/launch/hardware.launch.py @@ -1,7 +1,7 @@ from launch import LaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessStart -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PythonExpression from lbr_bringup.description import LBRDescriptionMixin from lbr_bringup.ros2_control import LBRROS2ControlMixin @@ -12,17 +12,20 @@ def generate_launch_description() -> LaunchDescription: # launch arguments ld.add_action(LBRDescriptionMixin.arg_model()) ld.add_action(LBRDescriptionMixin.arg_robot_name()) - ld.add_action(LBRDescriptionMixin.arg_port_id()) + ld.add_action(LBRROS2ControlMixin.arg_sys_cfg_pkg()) + ld.add_action(LBRROS2ControlMixin.arg_sys_cfg()) ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg_pkg()) ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg()) ld.add_action(LBRROS2ControlMixin.arg_ctrl()) - # static transform world -> robot_name/world + # static transform world -> _floating_link ld.add_action( LBRDescriptionMixin.node_static_tf( tf=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], parent="world", - child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]), + child=PythonExpression( + ["'", LaunchConfiguration("robot_name"), "' + '_floating_link'"] + ), ) ) @@ -36,7 +39,9 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(robot_state_publisher) # ros2 control node - ros2_control_node = LBRROS2ControlMixin.node_ros2_control(use_sim_time=False) + ros2_control_node = LBRROS2ControlMixin.node_ros2_control( + use_sim_time=False, robot_description=robot_description + ) ld.add_action(ros2_control_node) # joint state broad caster and controller on ros2 control node start diff --git a/lbr_bringup/launch/mock.launch.py b/lbr_bringup/launch/mock.launch.py index 3443431f..fc7c24f7 100644 --- a/lbr_bringup/launch/mock.launch.py +++ b/lbr_bringup/launch/mock.launch.py @@ -1,7 +1,7 @@ from launch import LaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessStart -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PythonExpression from lbr_bringup.description import LBRDescriptionMixin from lbr_bringup.ros2_control import LBRROS2ControlMixin @@ -16,12 +16,14 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg()) ld.add_action(LBRROS2ControlMixin.arg_ctrl()) - # static transform world -> robot_name/world + # static transform world -> _floating_link ld.add_action( LBRDescriptionMixin.node_static_tf( tf=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], parent="world", - child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]), + child=PythonExpression( + ["'", LaunchConfiguration("robot_name"), "' + '_floating_link'"] + ), ) ) @@ -35,7 +37,9 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(robot_state_publisher) # ros2 control node - ros2_control_node = LBRROS2ControlMixin.node_ros2_control(use_sim_time=False) + ros2_control_node = LBRROS2ControlMixin.node_ros2_control( + use_sim_time=False, robot_description=robot_description + ) ld.add_action(ros2_control_node) # joint state broad caster and controller on ros2 control node start diff --git a/lbr_bringup/launch/move_group.launch.py b/lbr_bringup/launch/move_group.launch.py index 7d71292a..fcfb19aa 100644 --- a/lbr_bringup/launch/move_group.launch.py +++ b/lbr_bringup/launch/move_group.launch.py @@ -46,8 +46,8 @@ def hidden_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: # RViz if desired rviz = RVizMixin.node_rviz( - rviz_config_pkg=f"{model}_moveit_config", - rviz_config="config/moveit.rviz", + rviz_cfg_pkg=f"{model}_moveit_config", + rviz_cfg="config/moveit.rviz", parameters=LBRMoveGroupMixin.params_rviz( moveit_configs=moveit_configs_builder.to_moveit_configs() ) diff --git a/lbr_bringup/launch/rviz.launch.py b/lbr_bringup/launch/rviz.launch.py index cacc2faf..51857d14 100644 --- a/lbr_bringup/launch/rviz.launch.py +++ b/lbr_bringup/launch/rviz.launch.py @@ -6,8 +6,8 @@ def generate_launch_description() -> LaunchDescription: ld = LaunchDescription() # launch arguments - ld.add_action(RVizMixin.arg_rviz_config()) - ld.add_action(RVizMixin.arg_rviz_config_pkg()) + ld.add_action(RVizMixin.arg_rviz_cfg()) + ld.add_action(RVizMixin.arg_rviz_cfg_pkg()) # rviz ld.add_action(RVizMixin.node_rviz()) diff --git a/lbr_bringup/lbr_bringup/description.py b/lbr_bringup/lbr_bringup/description.py index 8d612d5a..161bf12e 100644 --- a/lbr_bringup/lbr_bringup/description.py +++ b/lbr_bringup/lbr_bringup/description.py @@ -20,12 +20,21 @@ def param_robot_description( robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( "robot_name", default="lbr" ), - port_id: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( - "port_id", default="30200" - ), mode: Optional[Union[LaunchConfiguration, bool]] = LaunchConfiguration( "mode", default="mock" ), + system_config_path: Optional[ + Union[LaunchConfiguration, str] + ] = PathJoinSubstitution( + [ + FindPackageShare( + LaunchConfiguration("sys_cfg_pkg", default="lbr_description") + ), + LaunchConfiguration( + "sys_cfg", default="ros2_control/lbr_system_config.yaml" + ), + ] + ), ) -> Dict[str, str]: robot_description = { "robot_description": Command( @@ -43,10 +52,10 @@ def param_robot_description( ".xacro", " robot_name:=", robot_name, - " port_id:=", - port_id, " mode:=", mode, + " system_config_path:=", + system_config_path, ] ) } @@ -69,15 +78,6 @@ def arg_robot_name(default_value: str = "lbr") -> DeclareLaunchArgument: description="The robot's name.", ) - @staticmethod - def arg_port_id(default_value: str = "30200") -> DeclareLaunchArgument: - return DeclareLaunchArgument( - name="port_id", - default_value=default_value, - description="Port ID of the FRI communication. Valid in range [30200, 30209].\n" - "\tUsefull in multi-robot setups.", - ) - @staticmethod def arg_mode(default_value: str = "mock") -> DeclareLaunchArgument: return DeclareLaunchArgument( @@ -95,10 +95,6 @@ def arg_mode(default_value: str = "mock") -> DeclareLaunchArgument: def param_robot_name() -> Dict[str, LaunchConfiguration]: return {"robot_name": LaunchConfiguration("robot_name", default="lbr")} - @staticmethod - def param_port_id() -> Dict[str, LaunchConfiguration]: - return {"port_id": LaunchConfiguration("port_id", default="30200")} - @staticmethod def param_mode() -> Dict[str, LaunchConfiguration]: return {"mode": LaunchConfiguration("mode", default="mock")} diff --git a/lbr_bringup/lbr_bringup/moveit.py b/lbr_bringup/lbr_bringup/moveit.py index f2a69fbf..05685a3f 100644 --- a/lbr_bringup/lbr_bringup/moveit.py +++ b/lbr_bringup/lbr_bringup/moveit.py @@ -67,7 +67,10 @@ def moveit_configs_builder( f"urdf/{robot_name}/{robot_name}.xacro", ), ) - .planning_pipelines(default_planning_pipeline="ompl") + .planning_pipelines( + default_planning_pipeline="ompl", + pipelines=["ompl"], + ) ) @staticmethod diff --git a/lbr_bringup/lbr_bringup/ros2_control.py b/lbr_bringup/lbr_bringup/ros2_control.py index a5a58627..00f3a30e 100644 --- a/lbr_bringup/lbr_bringup/ros2_control.py +++ b/lbr_bringup/lbr_bringup/ros2_control.py @@ -11,7 +11,7 @@ class LBRROS2ControlMixin: def arg_ctrl_cfg_pkg() -> DeclareLaunchArgument: return DeclareLaunchArgument( name="ctrl_cfg_pkg", - default_value="lbr_ros2_control", + default_value="lbr_description", description="Controller configuration package. The package containing the ctrl_cfg.", ) @@ -19,7 +19,7 @@ def arg_ctrl_cfg_pkg() -> DeclareLaunchArgument: def arg_ctrl_cfg() -> DeclareLaunchArgument: return DeclareLaunchArgument( name="ctrl_cfg", - default_value="config/lbr_controllers.yaml", + default_value="ros2_control/lbr_controllers.yaml", description="Relative path from ctrl_cfg_pkg to the controllers.", ) @@ -30,14 +30,32 @@ def arg_ctrl() -> DeclareLaunchArgument: default_value="joint_trajectory_controller", description="Desired default controller. One of specified in ctrl_cfg.", choices=[ + "admittance_controller", "joint_trajectory_controller", "forward_position_controller", "lbr_joint_position_command_controller", "lbr_torque_command_controller", "lbr_wrench_command_controller", + "twist_controller", ], ) + @staticmethod + def arg_sys_cfg_pkg() -> DeclareLaunchArgument: + return DeclareLaunchArgument( + name="sys_cfg_pkg", + default_value="lbr_description", + description="Package containing the lbr_system_config.yaml file for FRI configurations.", + ) + + @staticmethod + def arg_sys_cfg() -> DeclareLaunchArgument: + return DeclareLaunchArgument( + name="sys_cfg", + default_value="ros2_control/lbr_system_config.yaml", + description="The relative path from sys_cfg_pkg to the lbr_system_config.yaml file.", + ) + @staticmethod def arg_use_sim_time() -> DeclareLaunchArgument: return DeclareLaunchArgument( @@ -54,6 +72,9 @@ def node_ros2_control( use_sim_time: Optional[Union[LaunchConfiguration, bool]] = LaunchConfiguration( "use_sim_time", default="false" ), + robot_description: Optional[ + Dict[str, str] + ] = {}, # required for certain ROS 2 controllers in Humble **kwargs, ) -> Node: return Node( @@ -65,14 +86,15 @@ def node_ros2_control( [ FindPackageShare( LaunchConfiguration( - "ctrl_cfg_pkg", default="lbr_ros2_control" + "ctrl_cfg_pkg", default="lbr_description" ) ), LaunchConfiguration( - "ctrl_cfg", default="config/lbr_controllers.yaml" + "ctrl_cfg", default="ros2_control/lbr_controllers.yaml" ), ] ), + robot_description, ], namespace=robot_name, remappings=[ @@ -122,10 +144,6 @@ def node_robot_state_publisher( parameters=[ robot_description, {"use_sim_time": use_sim_time}, - # use robot name as frame prefix - { - "frame_prefix": PathJoinSubstitution([robot_name, ""]) - }, # neat hack to add trailing slash, which is required by frame_prefix ], namespace=robot_name, **kwargs, diff --git a/lbr_bringup/lbr_bringup/rviz.py b/lbr_bringup/lbr_bringup/rviz.py index 824bee74..b85e0ffe 100644 --- a/lbr_bringup/lbr_bringup/rviz.py +++ b/lbr_bringup/lbr_bringup/rviz.py @@ -16,32 +16,32 @@ def arg_rviz(default_value: str = "false") -> DeclareLaunchArgument: ) @staticmethod - def arg_rviz_config_pkg( + def arg_rviz_cfg_pkg( default_value: str = "lbr_bringup", ) -> DeclareLaunchArgument: return DeclareLaunchArgument( - name="rviz_config_pkg", + name="rviz_cfg_pkg", default_value=default_value, - description="The RViz configuration file.", + description="The package containing the RViz configuration file.", ) @staticmethod - def arg_rviz_config( + def arg_rviz_cfg( default_value: str = "config/mock.rviz", ) -> DeclareLaunchArgument: return DeclareLaunchArgument( - name="rviz_config", + name="rviz_cfg", default_value=default_value, - description="The RViz configuration file.", + description="The RViz configuration file relative to rviz_cfg_pkg.", ) @staticmethod def node_rviz( - rviz_config_pkg: Optional[ - Union[LaunchConfiguration, str] - ] = LaunchConfiguration("rviz_config_pkg", default="lbr_bringup"), - rviz_config: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( - "rviz_config", default="config/mock.rviz" + rviz_cfg_pkg: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( + "rviz_cfg_pkg", default="lbr_bringup" + ), + rviz_cfg: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( + "rviz_cfg", default="config/mock.rviz" ), **kwargs, ) -> Node: @@ -53,8 +53,8 @@ def node_rviz( "-d", PathJoinSubstitution( [ - FindPackageShare(rviz_config_pkg), - rviz_config, + FindPackageShare(rviz_cfg_pkg), + rviz_cfg, ] ), ], diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index 5b351434..9639935a 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -2,7 +2,7 @@ lbr_bringup - 2.1.2 + 2.2.0 LBR launch files. mhubii Apache-2.0 @@ -17,6 +17,8 @@ lbr_description lbr_fri_ros2 lbr_ros2_control + moveit_planners_chomp + moveit_planners_ompl moveit_ros_move_group moveit_servo rclpy diff --git a/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml b/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml deleted file mode 100644 index 7df54a6d..00000000 --- a/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**/admittance_control: - ros__parameters: - base_link: "link_0" - end_effector_link: "link_ee" - f_ext_th: [2., 2., 2., 0.5, 0.5, 0.5] - dq_gains: [4.0, 4.0, 4.0, 4.0, 4.0, 4.0, 4.0] - dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] - exp_smooth: 0.95 diff --git a/lbr_ros2_control/config/lbr_system_parameters.yaml b/lbr_demos/lbr_demos_advanced_cpp/config/lbr_system_config.yaml similarity index 60% rename from lbr_ros2_control/config/lbr_system_parameters.yaml rename to lbr_demos/lbr_demos_advanced_cpp/config/lbr_system_config.yaml index 4ae4cbf2..165055de 100644 --- a/lbr_ros2_control/config/lbr_system_parameters.yaml +++ b/lbr_demos/lbr_demos_advanced_cpp/config/lbr_system_config.yaml @@ -7,20 +7,27 @@ hardware: port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection rt_prio: 80 # real-time priority for the control loop - pid_p: 0.1 # P gain for the joint position (useful for asynchronous control) - pid_i: 0.0 # I gain for the joint position command - pid_d: 0.0 # D gain for the joint position command - pid_i_max: 0.0 # max integral value for the joint position command - pid_i_min: 0.0 # min integral value for the joint position command - pid_antiwindup: false # enable antiwindup for the joint position command + # exponential moving average time constant for joint position commands [s]: + # Set tau > robot sample time for more smoothing on the commands + # Set tau << robot sample time for no smoothing on the commands + joint_position_tau: 0.2 command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] - external_torque_cutoff_frequency: 10 # low-pass filter for the external joint torque measurements [Hz] - measured_torque_cutoff_frequency: 10 # low-pass filter for the joint torque measurements [Hz] + # exponential moving average time constant for external joint torque measurements [s]: + # Set tau > robot sample time for more smoothing on the external torque measurements + # Set tau << robot sample time for more smoothing on the external torque measurements + external_torque_tau: 0.4 + # exponential moving average time constant for joint torque measurements [s]: + # Set tau > robot sample time for more smoothing on the raw torque measurements + # Set tau << robot sample time for more smoothing on the raw torque measurements + measured_torque_tau: 0.4 open_loop: true # KUKA works the best in open_loop control mode estimated_ft_sensor: # estimates the external force-torque from the external joint torque values - chain_root: link_0 - chain_tip: link_ee + enabled: true # whether to enable the force-torque estimation + update_rate: 100 # update rate for the force-torque estimation [Hz] (less or equal to controller manager update rate) + rt_prio: 30 # real-time priority for the force-torque estimation + chain_root: lbr_link_0 + chain_tip: lbr_link_ee damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian force_x_th: 2.0 # x-force threshold. Only if the force exceeds this value, the force will be considered force_y_th: 2.0 # y-force threshold. Only if the force exceeds this value, the force will be considered diff --git a/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst index 2cd26566..9fa5c943 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst +++ b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst @@ -8,14 +8,9 @@ lbr_demos_advanced_cpp :local: :backlinks: none -Admittance Controller ---------------------- -This demo implements a simple admittance controller. - -#. Client side configurations: - - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` +Twist Controller +---------------- +This demo uses the twist controller. #. Remote side configurations: @@ -35,16 +30,47 @@ This demo implements a simple admittance controller. .. code-block:: bash ros2 launch lbr_bringup hardware.launch.py \ - ctrl:=lbr_joint_position_command_controller \ + ctrl:=twist_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. Launch the `admittance_control `_:octicon:`link-external`: +#. Next, publish to the ``/lbr/command/twist`` topic: - .. code-block:: bash - - ros2 run lbr_demos_advanced_cpp admittance_control --ros-args \ - -r __ns:=/lbr \ - --params-file `ros2 pkg prefix lbr_demos_advanced_cpp`/share/lbr_demos_advanced_cpp/config/admittance_control.yaml + .. code-block:: bash + + ros2 topic pub \ + --rate 100 \ + /lbr/command/twist \ + geometry_msgs/msg/Twist \ + "{linear: {x: 0.0, y: 0.0, z: 0.05}, angular: {x: 0.0, y: 0.0, z: 0.0}}" + +#. If you ``Ctrl+C`` the publisher, the ``twist_controller`` throws an error as it expects a continuous stream of twist commands. + +Admittance Controller +--------------------- +This demo uses the admittance controller. + +#. Remote side configurations: + + #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` + + .. thumbnail:: ../../doc/img/applications_lbr_server.png + + #. Select + + - ``FRI send period``: ``10 ms`` + - ``IP address``: ``your configuration`` + - ``FRI control mode``: ``POSITION_CONTROL`` + - ``FRI client command mode``: ``POSITION`` + +#. Launch the robot driver (please note that a different system configuration file is used with heavier smoothing!): + + .. code-block:: bash + + ros2 launch lbr_bringup hardware.launch.py \ + ctrl:=admittance_controller \ + sys_cfg_pkg:=lbr_demos_advanced_cpp \ + sys_cfg:=config/lbr_system_config.yaml \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Now gently move the robot at the end-effector. @@ -55,8 +81,8 @@ kinematics to move the robot's end-effector along the z-axis in Cartesian space. #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_demos_advanced_cpp/package.xml b/lbr_demos/lbr_demos_advanced_cpp/package.xml index a1bcf8b9..8f955b23 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.2 + 2.2.0 Advanced C++ demos for the lbr_ros2_control. mhubii Apache-2.0 diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp b/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp index 8d38376a..d3d7c048 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp +++ b/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp @@ -32,8 +32,8 @@ class PoseControlNode : public LBRBasePositionCommandNode { return; } - this->declare_parameter("base_link", "link_0"); - this->declare_parameter("end_effector_link", "link_ee"); + this->declare_parameter("base_link", "lbr_link_0"); + this->declare_parameter("end_effector_link", "lbr_link_ee"); std::string root_link = this->get_parameter("base_link").as_string(); std::string tip_link = this->get_parameter("end_effector_link").as_string(); @@ -54,7 +54,7 @@ class PoseControlNode : public LBRBasePositionCommandNode { current_lbr_state_ = *lbr_state; double joint_position[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; - for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) { joint_position[i] = current_lbr_state_.measured_joint_position[i]; } current_pose_ = compute_fk_(joint_position); @@ -65,7 +65,7 @@ class PoseControlNode : public LBRBasePositionCommandNode { if (current_lbr_state_.session_state == KUKA::FRI::COMMANDING_ACTIVE) { KDL::JntArray current_joint_positions(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) { current_joint_positions(i) = current_lbr_state_.measured_joint_position[i]; } @@ -79,7 +79,7 @@ class PoseControlNode : public LBRBasePositionCommandNode { KDL::ChainFkSolverPos_recursive fk_solver = KDL::ChainFkSolverPos_recursive(chain_); KDL::JntArray joint_positions = KDL::JntArray(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) { joint_positions(i) = position_array_ptr[i]; } @@ -131,7 +131,7 @@ class PoseControlNode : public LBRBasePositionCommandNode { { RCLCPP_ERROR(this->get_logger(), "Inverse kinematics failed."); } else { - for (unsigned int i = 0; i < result_joint_positions.data.size(); i++) { + for (unsigned int i = 0; i < result_joint_positions.data.size(); ++i) { joint_position_command.joint_position[i] = result_joint_positions(i); } } diff --git a/lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml b/lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml index 4146d02f..1dbf503e 100644 --- a/lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml +++ b/lbr_demos/lbr_demos_advanced_py/config/admittance_control.yaml @@ -1,7 +1,7 @@ /**/admittance_control: ros__parameters: - base_link: "link_0" - end_effector_link: "link_ee" + base_link: "lbr_link_0" + end_effector_link: "lbr_link_ee" f_ext_th: [2., 2., 2., 0.5, 0.5, 0.5] dq_gains: [1., 1., 1., 1., 1., 1., 1.] dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] diff --git a/lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml b/lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml index 901d1d87..0f95f796 100644 --- a/lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml +++ b/lbr_demos/lbr_demos_advanced_py/config/admittance_rcm_control.yaml @@ -1,7 +1,7 @@ /**/admittance_rcm_control: ros__parameters: - base_link: "link_0" - end_effector_link: "link_ee" + base_link: "lbr_link_0" + end_effector_link: "lbr_link_ee" f_ext_th: [4.0, 4.0, 4.0, 1.0, 1.0, 1.0] dq_gains: [1., 1., 1., 1., 1., 1., 1.] dx_gains: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] diff --git a/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst b/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst index fb37b371..a87c5776 100644 --- a/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst +++ b/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst @@ -14,8 +14,8 @@ This demo implements a simple admittance controller. #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -54,8 +54,8 @@ This demo implements an admittance controller with a remote center of motion (RC #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py index eaccee81..d1aae155 100755 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_control_node.py @@ -12,8 +12,8 @@ def __init__(self, node_name: str = "admittance_control") -> None: super().__init__(node_name=node_name) # parameters - self.declare_parameter("base_link", "link_0") - self.declare_parameter("end_effector_link", "link_ee") + self.declare_parameter("base_link", "lbr_link_0") + self.declare_parameter("end_effector_link", "lbr_link_ee") self.declare_parameter("f_ext_th", [2.0, 2.0, 2.0, 0.5, 0.5, 0.5]) self.declare_parameter("dq_gains", [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) self.declare_parameter("dx_gains", [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py index 39aece33..d6875744 100644 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_controller.py @@ -8,8 +8,8 @@ class AdmittanceController(object): def __init__( self, robot_description: str, - base_link: str = "link_0", - end_effector_link: str = "link_ee", + base_link: str = "lbr_link_0", + end_effector_link: str = "lbr_link_ee", f_ext_th: np.ndarray = np.array([2.0, 2.0, 2.0, 0.5, 0.5, 0.5]), dq_gains: np.ndarray = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]), dx_gains: np.ndarray = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]), diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py index 55eb5bac..cc99dfbd 100644 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_control_node.py @@ -12,8 +12,8 @@ def __init__(self, node_name: str = "admittance_rcm_control") -> None: super().__init__(node_name) # declare and get parameters - self.declare_parameter("base_link", "link_0") - self.declare_parameter("end_effector_link", "link_ee") + self.declare_parameter("base_link", "lbr_link_0") + self.declare_parameter("end_effector_link", "lbr_link_ee") self.declare_parameter("f_ext_th", [4.0, 4.0, 4.0, 1.0, 1.0, 1.0]) self.declare_parameter("dq_gains", [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) self.declare_parameter("dx_gains", [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) diff --git a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_controller.py b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_controller.py index 95fd3b18..4c9b5486 100644 --- a/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_controller.py +++ b/lbr_demos/lbr_demos_advanced_py/lbr_demos_advanced_py/admittance_rcm_controller.py @@ -6,8 +6,8 @@ class AdmittanceRCMController: def __init__( self, robot_description: str, - base_link: str = "link_0", - end_effector_link: str = "link_ee", + base_link: str = "lbr_link_0", + end_effector_link: str = "lbr_link_ee", ): self._robot = optas.RobotModel( urdf_string=robot_description, time_derivs=[0, 1] diff --git a/lbr_demos/lbr_demos_advanced_py/package.xml b/lbr_demos/lbr_demos_advanced_py/package.xml index c485e4ac..f7098ccc 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.2 + 2.2.0 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 c465713a..e49a6d12 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.2", + version="2.2.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), @@ -18,7 +18,7 @@ maintainer="mhubii", maintainer_email="m.huber_1994@hotmail.de", description="Advanced Python demos for the lbr_ros2_control.", - license="MIT", + license="Apache-2.0", tests_require=["pytest"], entry_points={ "console_scripts": [ diff --git a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst index dafaa698..d11bf3f2 100644 --- a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst +++ b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst @@ -14,8 +14,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -72,8 +72,8 @@ Hardware ~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -96,8 +96,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays #. Client side configurations: - #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_parameters.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -134,8 +134,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays #. Client side configurations: - #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_parameters.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_demos_cpp/package.xml b/lbr_demos/lbr_demos_cpp/package.xml index 6ed61d1a..7376b480 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.2 + 2.2.0 C++ demos for lbr_ros2_control. mhubii Apache-2.0 diff --git a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst index c15ed340..901a6f15 100644 --- a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst +++ b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst @@ -14,8 +14,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -72,8 +72,8 @@ Hardware ~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -96,8 +96,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays #. Client side configurations: - #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_parameters.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``torque`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -134,8 +134,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays #. Client side configurations: - #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_parameters.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``wrench`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``500`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_demos_py/package.xml b/lbr_demos/lbr_demos_py/package.xml index 3d0ca085..d0e55a3a 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.2 + 2.2.0 Python demos for lbr_ros2_control. mhubii Apache-2.0 diff --git a/lbr_demos/lbr_demos_py/setup.py b/lbr_demos/lbr_demos_py/setup.py index 76198e83..b16641b8 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.2", + version="2.2.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), @@ -15,7 +15,7 @@ maintainer="mhubii", maintainer_email="m.huber_1994@hotmail.de", description="Python demos for lbr_ros2_control.", - license="MIT", + license="Apache-2.0", tests_require=["pytest"], entry_points={ "console_scripts": [ diff --git a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst index 9b7d394a..ac21da03 100644 --- a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst +++ b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst @@ -62,8 +62,8 @@ MoveIt Servo - Hardware ~~~~~~~~~~~~~~~~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: @@ -122,8 +122,8 @@ MoveIt via RViz - Hardware ~~~~~~~~~~~~~~~~~~~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_moveit/package.xml b/lbr_demos/lbr_moveit/package.xml index f2116fe8..44f3cb60 100644 --- a/lbr_demos/lbr_moveit/package.xml +++ b/lbr_demos/lbr_moveit/package.xml @@ -2,7 +2,7 @@ lbr_moveit - 2.1.2 + 2.2.0 MoveIt demos for the LBRs mhubii Apache-2.0 diff --git a/lbr_demos/lbr_moveit/setup.py b/lbr_demos/lbr_moveit/setup.py index a4d342ce..65adda1c 100644 --- a/lbr_demos/lbr_moveit/setup.py +++ b/lbr_demos/lbr_moveit/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version="2.1.2", + version="2.2.0", packages=find_packages(exclude=["test"]), data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst index 5d907950..46e27a17 100644 --- a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst +++ b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst @@ -41,8 +41,8 @@ Hardware ~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` - #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_config.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` #. Remote side configurations: diff --git a/lbr_demos/lbr_moveit_cpp/package.xml b/lbr_demos/lbr_moveit_cpp/package.xml index 85d80d66..245ca34e 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.2 + 2.2.0 Demo for using MoveIt C++ API. mhubii Apache-2.0 diff --git a/lbr_description/CMakeLists.txt b/lbr_description/CMakeLists.txt index 6debb078..e54c291e 100644 --- a/lbr_description/CMakeLists.txt +++ b/lbr_description/CMakeLists.txt @@ -11,7 +11,7 @@ find_package(ament_cmake_pytest REQUIRED) # install install( - DIRECTORY gazebo meshes urdf + DIRECTORY gazebo meshes ros2_control urdf DESTINATION share/${PROJECT_NAME} ) diff --git a/lbr_description/gazebo/lbr_gazebo.xacro b/lbr_description/gazebo/lbr_gazebo.xacro index a828c0d5..c9539f90 100644 --- a/lbr_description/gazebo/lbr_gazebo.xacro +++ b/lbr_description/gazebo/lbr_gazebo.xacro @@ -6,7 +6,7 @@ - $(find lbr_ros2_control)/config/lbr_controllers.yaml + $(find lbr_description)/ros2_control/lbr_controllers.yaml /${robot_name} @@ -29,20 +29,20 @@ - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/lbr_description/package.xml b/lbr_description/package.xml index a3b807df..4e14f30b 100644 --- a/lbr_description/package.xml +++ b/lbr_description/package.xml @@ -2,7 +2,7 @@ lbr_description - 2.1.2 + 2.2.0 KUKA LBR description files mhubii Apache-2.0 diff --git a/lbr_description/ros2_control/lbr_controllers.yaml b/lbr_description/ros2_control/lbr_controllers.yaml new file mode 100644 index 00000000..9a2a1a3d --- /dev/null +++ b/lbr_description/ros2_control/lbr_controllers.yaml @@ -0,0 +1,124 @@ +# Use of /** so that the configurations hold for controller +# managers regardless of their namespace. Usefull in multi-robot setups. +/**/controller_manager: + ros__parameters: + update_rate: 100 + + # ROS 2 control broadcasters + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + force_torque_broadcaster: + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + + # LBR ROS 2 control broadcasters + lbr_state_broadcaster: + type: lbr_ros2_control/LBRStateBroadcaster + + # ROS 2 control controllers + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + forward_position_controller: + type: position_controllers/JointGroupPositionController + + # LBR ROS 2 control controllers + admittance_controller: + type: lbr_ros2_control/AdmittanceController + + lbr_joint_position_command_controller: + type: lbr_ros2_control/LBRJointPositionCommandController + + lbr_torque_command_controller: + type: lbr_ros2_control/LBRTorqueCommandController + + lbr_wrench_command_controller: + type: lbr_ros2_control/LBRWrenchCommandController + + twist_controller: + type: lbr_ros2_control/TwistController + +# ROS 2 control broadcasters +/**/force_torque_broadcaster: + ros__parameters: + frame_id: lbr_link_ee # namespace: https://github.com/ros2/rviz/issues/1103 + sensor_name: estimated_ft_sensor + +# LBR ROS 2 control broadcasters +/**/lbr_state_broadcaster: + ros__parameters: + robot_name: lbr + +# ROS 2 control controllers +/**/joint_trajectory_controller: + ros__parameters: + joints: + - lbr_A1 + - lbr_A2 + - lbr_A3 + - lbr_A4 + - lbr_A5 + - lbr_A6 + - lbr_A7 + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 50.0 + action_monitor_rate: 20.0 + +/**/forward_position_controller: + ros__parameters: + joints: + - lbr_A1 + - lbr_A2 + - lbr_A3 + - lbr_A4 + - lbr_A5 + - lbr_A6 + - lbr_A7 + interface_name: position + +# LBR ROS 2 control controllers +/**/lbr_joint_position_command_controller: + ros__parameters: + robot_name: lbr + +/**/lbr_torque_command_controller: + ros__parameters: + robot_name: lbr + +/**/lbr_wrench_command_controller: + ros__parameters: + robot_name: lbr + +/**/admittance_controller: + ros__parameters: + robot_name: lbr + admittance: + mass: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] + damping: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + stiffness: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + inv_jac_ctrl: + chain_root: lbr_link_0 + chain_tip: lbr_link_ee + damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian + max_linear_velocity: 2.0 # maximum linear velocity + max_angular_velocity: 2.0 # maximum linear acceleration + joint_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # joint gains + cartesian_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # cartesian gains + +/**/twist_controller: + ros__parameters: + robot_name: lbr + inv_jac_ctrl: + chain_root: lbr_link_0 + chain_tip: lbr_link_ee + twist_in_tip_frame: true # if true, the twist command is expressed in the tip frame, otherwise in the root frame + damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian + max_linear_velocity: 0.1 # maximum linear velocity + max_angular_velocity: 0.1 # maximum linear acceleration + joint_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # joint gains + cartesian_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # cartesian gains + timeout: 0.2 # stop controller if no command is received within this time [s] diff --git a/lbr_description/ros2_control/lbr_system_config.yaml b/lbr_description/ros2_control/lbr_system_config.yaml new file mode 100644 index 00000000..8f96cf0d --- /dev/null +++ b/lbr_description/ros2_control/lbr_system_config.yaml @@ -0,0 +1,37 @@ +# these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface +hardware: + fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro + major_version: 1 + minor_version: 15 + client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench] + port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups + remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection + rt_prio: 80 # real-time priority for the control loop + # exponential moving average time constant for joint position commands [s]: + # Set tau > robot sample time for more smoothing on the commands + # Set tau << robot sample time for no smoothing on the commands + joint_position_tau: 0.04 + command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] + # exponential moving average time constant for external joint torque measurements [s]: + # Set tau > robot sample time for more smoothing on the external torque measurements + # Set tau << robot sample time for more smoothing on the external torque measurements + external_torque_tau: 0.04 + # exponential moving average time constant for joint torque measurements [s]: + # Set tau > robot sample time for more smoothing on the raw torque measurements + # Set tau << robot sample time for more smoothing on the raw torque measurements + measured_torque_tau: 0.04 + open_loop: true # KUKA works the best in open_loop control mode + +estimated_ft_sensor: # estimates the external force-torque from the external joint torque values + enabled: true # whether to enable the force-torque estimation + update_rate: 100 # update rate for the force-torque estimation [Hz] (less or equal to controller manager update rate) + rt_prio: 30 # real-time priority for the force-torque estimation + chain_root: lbr_link_0 + chain_tip: lbr_link_ee + damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian + force_x_th: 2.0 # x-force threshold. Only if the force exceeds this value, the force will be considered + force_y_th: 2.0 # y-force threshold. Only if the force exceeds this value, the force will be considered + force_z_th: 2.0 # z-force threshold. Only if the force exceeds this value, the force will be considered + torque_x_th: 0.5 # x-torque threshold. Only if the torque exceeds this value, the torque will be considered + torque_y_th: 0.5 # y-torque threshold. Only if the torque exceeds this value, the torque will be considered + torque_z_th: 0.5 # z-torque threshold. Only if the torque exceeds this value, the torque will be considered diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_description/ros2_control/lbr_system_interface.xacro similarity index 65% rename from lbr_ros2_control/config/lbr_system_interface.xacro rename to lbr_description/ros2_control/lbr_system_interface.xacro index 23738b9a..eb9c1480 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_description/ros2_control/lbr_system_interface.xacro @@ -1,12 +1,12 @@ + params="robot_name mode joint_limits system_config_path"> - + - + @@ -21,22 +21,17 @@ lbr_ros2_control::SystemInterface - ${system_parameters['hardware']['fri_client_sdk']['major_version']} - ${system_parameters['hardware']['fri_client_sdk']['minor_version']} - ${system_parameters['hardware']['client_command_mode']} - ${system_parameters['hardware']['port_id']} - ${system_parameters['hardware']['remote_host']} - ${system_parameters['hardware']['rt_prio']} - ${system_parameters['hardware']['pid_p']} - ${system_parameters['hardware']['pid_i']} - ${system_parameters['hardware']['pid_d']} - ${system_parameters['hardware']['pid_i_max']} - ${system_parameters['hardware']['pid_i_min']} - ${system_parameters['hardware']['pid_antiwindup']} - ${system_parameters['hardware']['command_guard_variant']} - ${system_parameters['hardware']['external_torque_cutoff_frequency']} - ${system_parameters['hardware']['measured_torque_cutoff_frequency']} - ${system_parameters['hardware']['open_loop']} + ${system_config['hardware']['fri_client_sdk']['major_version']} + ${system_config['hardware']['fri_client_sdk']['minor_version']} + ${system_config['hardware']['client_command_mode']} + ${system_config['hardware']['port_id']} + ${system_config['hardware']['remote_host']} + ${system_config['hardware']['rt_prio']} + ${system_config['hardware']['joint_position_tau']} + ${system_config['hardware']['command_guard_variant']} + ${system_config['hardware']['external_torque_tau']} + ${system_config['hardware']['measured_torque_tau']} + ${system_config['hardware']['open_loop']} @@ -61,21 +56,26 @@ - ${system_parameters['estimated_ft_sensor']['chain_root']} - ${system_parameters['estimated_ft_sensor']['chain_tip']} - ${system_parameters['estimated_ft_sensor']['damping']} - ${system_parameters['estimated_ft_sensor']['force_x_th']} - ${system_parameters['estimated_ft_sensor']['force_y_th']} - ${system_parameters['estimated_ft_sensor']['force_z_th']} - ${system_parameters['estimated_ft_sensor']['torque_x_th']} - ${system_parameters['estimated_ft_sensor']['torque_y_th']} - ${system_parameters['estimated_ft_sensor']['torque_z_th']} - - - - - - + ${system_config['estimated_ft_sensor']['enabled']} + ${system_config['estimated_ft_sensor']['update_rate']} + ${system_config['estimated_ft_sensor']['rt_prio']} + ${system_config['estimated_ft_sensor']['chain_root']} + ${system_config['estimated_ft_sensor']['chain_tip']} + ${system_config['estimated_ft_sensor']['damping']} + ${system_config['estimated_ft_sensor']['force_x_th']} + ${system_config['estimated_ft_sensor']['force_y_th']} + ${system_config['estimated_ft_sensor']['force_z_th']} + ${system_config['estimated_ft_sensor']['torque_x_th']} + ${system_config['estimated_ft_sensor']['torque_y_th']} + ${system_config['estimated_ft_sensor']['torque_z_th']} + + + + + + + + @@ -114,7 +114,8 @@ ${max_position} ${max_velocity} ${max_torque} - + @@ -124,43 +125,43 @@ - - - - - - - + name="system_config_path" + default="$(find lbr_description)/ros2_control/lbr_system_config.yaml" /> - - + + - - - + between _floating_link and robot_name_link_0--> + + + + system_config_path="$(arg system_config_path)" /> \ No newline at end of file diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.xacro index 712fc099..cc0fbc64 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.xacro @@ -3,11 +3,11 @@ + params="robot_name:=^|lbr mode:=^|mock system_config_path:=^|'$(find lbr_description)/ros2_control/lbr_system_config.yaml'"> - + + system_config_path="${system_config_path}" /> - + @@ -47,10 +48,10 @@ - + - - + + - + @@ -81,10 +82,10 @@ - + - - + + - + @@ -115,10 +116,10 @@ - + - - + + - + @@ -149,10 +150,10 @@ - + - - + + - + @@ -183,10 +184,10 @@ - + - - + + - + @@ -217,10 +218,10 @@ - + - - + + - + @@ -251,10 +252,10 @@ - + - - + + - + @@ -284,13 +285,13 @@ - - - + + + - + \ No newline at end of file diff --git a/lbr_description/urdf/iiwa7/iiwa7.xacro b/lbr_description/urdf/iiwa7/iiwa7.xacro index 501160d4..fb9aca5e 100644 --- a/lbr_description/urdf/iiwa7/iiwa7.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7.xacro @@ -9,22 +9,22 @@ + name="system_config_path" + default="$(find lbr_description)/ros2_control/lbr_system_config.yaml" /> - - + + - - - + between _floating_link and robot_name_link_0--> + + + + system_config_path="$(arg system_config_path)" /> \ No newline at end of file diff --git a/lbr_description/urdf/iiwa7/iiwa7_description.xacro b/lbr_description/urdf/iiwa7/iiwa7_description.xacro index d177bb80..65374c44 100644 --- a/lbr_description/urdf/iiwa7/iiwa7_description.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7_description.xacro @@ -3,11 +3,11 @@ + params="robot_name:=^|lbr mode:=^|mock system_config_path:=^|'$(find lbr_description)/ros2_control/lbr_system_config.yaml'"> - + + system_config_path="${system_config_path}" /> - + @@ -47,10 +48,10 @@ - + - - + + - + @@ -81,10 +82,10 @@ - + - - + + - + @@ -115,10 +116,10 @@ - + - - + + - + @@ -149,10 +150,10 @@ - + - - + + - + @@ -183,10 +184,10 @@ - + - - + + - + @@ -217,10 +218,10 @@ - + - - + + - + @@ -252,10 +253,10 @@ - + - - + + - + @@ -285,13 +286,13 @@ - - - + + + - + \ No newline at end of file diff --git a/lbr_description/urdf/med14/med14.xacro b/lbr_description/urdf/med14/med14.xacro index 89881aaa..8cfa9566 100644 --- a/lbr_description/urdf/med14/med14.xacro +++ b/lbr_description/urdf/med14/med14.xacro @@ -9,22 +9,22 @@ + name="system_config_path" + default="$(find lbr_description)/ros2_control/lbr_system_config.yaml" /> - - + + - - - + between _floating_link and robot_name_link_0--> + + + + system_config_path="$(arg system_config_path)" /> \ No newline at end of file diff --git a/lbr_description/urdf/med14/med14_description.xacro b/lbr_description/urdf/med14/med14_description.xacro index 0eac46fe..b5f55d5f 100644 --- a/lbr_description/urdf/med14/med14_description.xacro +++ b/lbr_description/urdf/med14/med14_description.xacro @@ -3,11 +3,11 @@ + params="robot_name:=^|lbr mode:=^|mock system_config_path:=^|'$(find lbr_description)/ros2_control/lbr_system_config.yaml'"> - + + system_config_path="${system_config_path}" /> - + @@ -48,10 +49,10 @@ - + - - + + - + @@ -82,10 +83,10 @@ - + - - + + - + @@ -116,10 +117,10 @@ - + - - + + - + @@ -150,10 +151,10 @@ - + - - + + - + @@ -184,10 +185,10 @@ - + - - + + - + @@ -218,10 +219,10 @@ - + - - + + - + @@ -252,10 +253,10 @@ - + - - + + - + @@ -285,13 +286,13 @@ - - - + + + - + \ No newline at end of file diff --git a/lbr_description/urdf/med7/med7.xacro b/lbr_description/urdf/med7/med7.xacro index e34caeb8..81e2a72d 100644 --- a/lbr_description/urdf/med7/med7.xacro +++ b/lbr_description/urdf/med7/med7.xacro @@ -9,22 +9,22 @@ + name="system_config_path" + default="$(find lbr_description)/ros2_control/lbr_system_config.yaml" /> - - + + - - - + between _floating_link and robot_name_link_0--> + + + + system_config_path="$(arg system_config_path)" /> \ No newline at end of file diff --git a/lbr_description/urdf/med7/med7_description.xacro b/lbr_description/urdf/med7/med7_description.xacro index 58c885c4..344fc4f7 100644 --- a/lbr_description/urdf/med7/med7_description.xacro +++ b/lbr_description/urdf/med7/med7_description.xacro @@ -3,11 +3,11 @@ + params="robot_name:=^|lbr mode:=^|mock system_config_path:=^|'$(find lbr_description)/ros2_control/lbr_system_config.yaml'"> - + + system_config_path="${system_config_path}" /> - + @@ -48,10 +49,10 @@ - + - - + + - + @@ -82,10 +83,10 @@ - + - - + + - + @@ -116,10 +117,10 @@ - + - - + + - + @@ -150,10 +151,10 @@ - + - - + + - + @@ -184,10 +185,10 @@ - + - - + + - + @@ -218,10 +219,10 @@ - + - - + + - + @@ -252,10 +253,10 @@ - + - - + + - + @@ -285,13 +286,13 @@ - - - + + + - + \ No newline at end of file diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 47a55f94..a26f445b 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -11,10 +11,10 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(control_toolbox REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3) find_package(FRIClient REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(kdl_parser REQUIRED) find_package(lbr_fri_idl REQUIRED) find_package(orocos_kdl_vendor REQUIRED) @@ -33,8 +33,11 @@ add_library(lbr_fri_ros2 src/app.cpp src/async_client.cpp src/command_guard.cpp + src/control.cpp src/filters.cpp src/ft_estimator.cpp + src/kinematics.cpp + src/worker.cpp ) target_include_directories(lbr_fri_ros2 @@ -44,8 +47,8 @@ target_include_directories(lbr_fri_ros2 ) ament_target_dependencies(lbr_fri_ros2 - control_toolbox Eigen3 + geometry_msgs kdl_parser lbr_fri_idl orocos_kdl_vendor @@ -59,10 +62,10 @@ target_link_libraries(lbr_fri_ros2 ament_export_targets(lbr_fri_ros2_export HAS_LIBRARY_TARGET) ament_export_dependencies( - control_toolbox eigen3_cmake_module Eigen3 FRIClient + geometry_msgs kdl_parser lbr_fri_idl orocos_kdl_vendor diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index f66a8af0..aea97da5 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -1,40 +1,42 @@ #ifndef LBR_FRI_ROS2__APP_HPP_ #define LBR_FRI_ROS2__APP_HPP_ -#include #include -#include +#include #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" -#include "realtime_tools/thread_priority.hpp" #include "friClientApplication.h" #include "friUdpConnection.h" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/formatting.hpp" +#include "lbr_fri_ros2/worker.hpp" namespace lbr_fri_ros2 { -class App { -protected: - static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::App"; - +class App : public Worker { + /** + * @brief This clas provides utilities to run the KUKA::FRI::ClientApplication asynchronously. + * Note that the rate at which the application runs is determined by the robot. This is because + * the run_thread_ uses blocking function calls from the FRI client SDK, i.e. + * KUKA::FRI::ClientApplication::step() (this is by KUKA's design). + * + */ public: App(const std::shared_ptr async_client_ptr); ~App(); bool open_udp_socket(const int &port_id = 30200, const char *const remote_host = NULL); bool close_udp_socket(); - void run_async(int rt_prio = 80); - void request_stop(); + void run_async(int rt_prio = 80) override; + + inline std::string LOGGER_NAME() const override { return "lbr_fri_ros2::App"; }; protected: + void perform_work_() override; bool valid_port_(const int &port_id); - std::atomic_bool should_stop_, running_; - std::thread run_thread_; - std::shared_ptr async_client_ptr_; std::unique_ptr connection_ptr_; std::unique_ptr app_ptr_; diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp index db194729..f44f33a8 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp @@ -11,7 +11,6 @@ #include "friClientVersion.h" #include "friLBRClient.h" -#include "lbr_fri_ros2/filters.hpp" #include "lbr_fri_ros2/formatting.hpp" #include "lbr_fri_ros2/interfaces/base_command.hpp" #include "lbr_fri_ros2/interfaces/position_command.hpp" @@ -27,10 +26,10 @@ class AsyncClient : public KUKA::FRI::LBRClient { public: AsyncClient() = delete; AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mode, - const PIDParameters &pid_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant, - const StateInterfaceParameters &state_interface_parameters = {10.0, 10.0}, + const StateInterfaceParameters &state_interface_parameters = {0.04, 0.04}, const bool &open_loop = true); inline std::shared_ptr get_command_interface() { diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp index 9b963266..c4a86b05 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp @@ -17,13 +17,10 @@ #include "lbr_fri_idl/msg/lbr_command.hpp" #include "lbr_fri_idl/msg/lbr_state.hpp" #include "lbr_fri_ros2/formatting.hpp" +#include "lbr_fri_ros2/types.hpp" namespace lbr_fri_ros2 { struct CommandGuardParameters { - // ROS IDL types - using jnt_array_t = std::array; - using jnt_name_array_t = std::array; - jnt_name_array_t joint_names; /**< Joint names.*/ jnt_array_t min_positions{0., 0., 0., 0., 0., 0., 0.}; /**< Minimum joint positions [rad].*/ jnt_array_t max_positions{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint positions [rad].*/ @@ -35,14 +32,6 @@ class CommandGuard { protected: static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::CommandGuard"; - // ROS IDL types - using idl_command_t = lbr_fri_idl::msg::LBRCommand; - using const_idl_command_t_ref = const idl_command_t &; - using idl_state_t = lbr_fri_idl::msg::LBRState; - using const_idl_state_t_ref = const idl_state_t &; - using jnt_array_t = idl_command_t::_joint_position_type; - using const_jnt_array_t_ref = const jnt_array_t &; - public: CommandGuard() = default; CommandGuard(const CommandGuardParameters &command_guard_parameters); diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp new file mode 100644 index 00000000..128f9b85 --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/control.hpp @@ -0,0 +1,108 @@ +#ifndef LBR_FRI_ROS2__CONTROL_HPP_ +#define LBR_FRI_ROS2__CONTROL_HPP_ + +#include +#include +#include +#include + +#include "eigen3/Eigen/Core" +#include "geometry_msgs/msg/twist.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" + +#include "lbr_fri_ros2/kinematics.hpp" +#include "lbr_fri_ros2/pinv.hpp" +#include "lbr_fri_ros2/types.hpp" + +namespace lbr_fri_ros2 { +struct InvJacCtrlParameters { + std::string chain_root = "lbr_link_0"; + std::string chain_tip = "lbr_link_ee"; + bool twist_in_tip_frame = true; + double damping = 0.2; + double max_linear_velocity = 0.1; + double max_angular_velocity = 0.1; + jnt_array_t joint_gains = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + cart_array_t cartesian_gains = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; +}; + +class InvJacCtrlImpl { +protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::InvJacCtrlImpl"; + +public: + InvJacCtrlImpl(const std::string &robot_description, const InvJacCtrlParameters ¶meters); + + void compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target, const_jnt_array_t_ref q, + jnt_array_t_ref dq); + void compute(const_cart_array_t_ref twist_target, const_jnt_array_t_ref q, jnt_array_t_ref dq); + void compute(const Eigen::Matrix &twist_target, const_jnt_array_t_ref q, + jnt_array_t_ref dq); + + inline const std::unique_ptr &get_kinematics_ptr() const { return kinematics_ptr_; }; + + void log_info() const; + +protected: + void compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq); + void set_all_zero_(); + + InvJacCtrlParameters parameters_; + + jnt_array_t q_; + std::unique_ptr kinematics_ptr_; + Eigen::Matrix jacobian_inv_; + Eigen::Matrix twist_target_; + Eigen::Matrix q_gains_; + Eigen::Matrix x_gains_; +}; + +struct AdmittanceParameters { + AdmittanceParameters() = delete; + AdmittanceParameters(const_cart_array_t_ref m = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}, + const_cart_array_t_ref b = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}, + const_cart_array_t_ref k = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}) + : m(m), b(b), k(k) { + if (std::any_of(m.cbegin(), m.cend(), [](const double &m_i) { return m_i <= 0.0; })) { + throw std::runtime_error("Mass must be positive and greater zero."); + } + if (std::any_of(b.cbegin(), b.cend(), [](const double &b_i) { return b_i < 0.0; })) { + throw std::runtime_error("Damping must be positive."); + } + if (std::any_of(k.cbegin(), k.cend(), [](const double &k_i) { return k_i < 0.0; })) { + throw std::runtime_error("Stiffness must be positive."); + } + } + + cart_array_t m = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; + cart_array_t b = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}; + cart_array_t k = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; +}; + +class AdmittanceImpl { +protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::AdmittanceImpl"; + +public: + AdmittanceImpl() = delete; + AdmittanceImpl(const AdmittanceParameters ¶meters) : parameters_(parameters) { + m_ = Eigen::Map>(parameters_.m.data()); + b_ = Eigen::Map>(parameters_.b.data()); + k_ = Eigen::Map>(parameters_.k.data()); + } + + void compute(const Eigen::Matrix &f_ext, + const Eigen::Matrix &x, + const Eigen::Matrix &dx, + Eigen::Matrix &ddx); + + void log_info() const; + +protected: + AdmittanceParameters parameters_; + + Eigen::Matrix m_, b_, k_; +}; +} // namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__CONTROL_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp index f14d5915..3773be57 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp @@ -3,56 +3,71 @@ #include #include -#include #include +#include -#include "control_toolbox/filters.hpp" -#include "control_toolbox/pid_ros.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" -#include "friLBRClient.h" - -#include "lbr_fri_idl/msg/lbr_state.hpp" +#include "lbr_fri_ros2/types.hpp" namespace lbr_fri_ros2 { class ExponentialFilter { public: /** - * @brief Construct a new Exponential Filter object. Performs exponential smoothing with a - * #cutoff_frequency_ according to - * https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency. + * @brief Construct a new ExponentialFilter object. * */ ExponentialFilter(); /** - * @brief Construct a new Exponential Filter object. Performs exponential smoothing with a - * #cutoff_frequency_ according to - * https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency. + * @brief Construct a new ExponentialFilter object. + * + * @param[in] tau Time constant in seconds. + */ + ExponentialFilter(const double &tau); + + /** + * @brief Initialize the filter from members. Computes the new #alpha_ following + * alpha = 1.0 - exp(-sample_time / tau). + * + * If tau << sample_time => alpha -> 1 (very fast response, no smoothing) + * If tau >> sample_time => alpha -> 0 (very slow response, heavy smoothing) * - * @param[in] cutoff_frequency Frequency in Hz. * @param[in] sample_time Sample time in seconds. */ - ExponentialFilter(const double &cutoff_frequency, const double &sample_time); + void initialize(const double &sample_time); /** - * @brief Compute the exponential smoothing using the control_toolbox - * https://github.com/ros-controls/control_toolbox. + * @brief Initialize the filter. Computes the new #alpha_ following + * alpha = 1.0 - exp(-sample_time / tau). + * + * If tau << sample_time => alpha -> 1 (very fast response, no smoothing) + * If tau >> sample_time => alpha -> 0 (very slow response, heavy smoothing) + * + * @param[in] tau Time constant in seconds. + * @param[in] sample_time Sample time in seconds. + */ + void initialize(const double &tau, const double &sample_time); + + /** + * @brief Compute the exponential smoothing. Internally computes the new smoothed value following + * smoothed = alpha * current + (1 - alpha) * previous. * * @param[in] current The current value. * @param[in] previous The previous smoothed value. * @return double The returned smoothed value. */ inline double compute(const double ¤t, const double &previous) { - return filters::exponentialSmoothing(current, previous, alpha_); + return alpha_ * current + (1.0 - alpha_) * previous; }; /** - * @brief Set the cutoff frequency object. Internally computes the new #alpha_. + * @brief Get the time constant #tau_. * - * @param[in] cutoff_frequency Frequency in Hz. - * @param[in] sample_time Sample time in seconds. + * @return const double& */ - void set_cutoff_frequency(const double &cutoff_frequency, const double &sample_time); + inline const double &get_tau() const { return tau_; }; /** * @brief Get #sample_time_. @@ -69,16 +84,6 @@ class ExponentialFilter { inline const double &get_alpha() const { return alpha_; }; protected: - /** - * @brief Compute alpha given the cutoff frequency and the sample time. - * - * @param[in] cutoff_frequency Frequency in Hz. - * @param[in] sample_time Sample time in seconds. - * @return double Alpha based on - * https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency. - */ - double compute_alpha_(const double &cutoff_frequency, const double &sample_time); - /** * @brief Validate alpha in [0, 1]. * @@ -88,56 +93,30 @@ class ExponentialFilter { */ bool validate_alpha_(const double &alpha); - double cutoff_frequency_; /**< Frequency in Hz.*/ - double sample_time_; /**< Sample time in seconds.*/ - double - alpha_; /**< Alpha parameter based on - https://dsp.stackexchange.com/questions/40462/exponential-moving-average-cut-off-frequency.*/ + double tau_; /**< Time constant in seconds.*/ + double sample_time_; /**< Sample time in seconds.*/ + double alpha_; /**< Smoothing parameter in [0, 1].*/ }; class JointExponentialFilterArray { - using value_array_t = std::array; +protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::JointExponentialFilterArray"; public: JointExponentialFilterArray() = default; + JointExponentialFilterArray(const double &tau); - void compute(const double *const current, value_array_t &previous); - void initialize(const double &cutoff_frequency, const double &sample_time); + void compute(const double *const current, jnt_array_t_ref previous); + void compute(const_jnt_array_t_ref current, jnt_array_t_ref previous); + void initialize(const double &sample_time); + void initialize(const double &tau, const double &sample_time); inline const bool &is_initialized() const { return initialized_; }; -protected: - bool initialized_{false}; /**< True if initialized.*/ - ExponentialFilter exponential_filter_; /**< Exponential filter applied to all joints.*/ -}; - -struct PIDParameters { - double p{0.0}; /**< Proportional gain.*/ - double i{0.0}; /**< Integral gain.*/ - double d{0.0}; /**< Derivative gain.*/ - double i_max{0.0}; /**< Maximum integral value.*/ - double i_min{0.0}; /**< Minimum integral value.*/ - bool antiwindup{false}; /**< Antiwindup enabled.*/ -}; - -class JointPIDArray { -protected: - static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::JointPIDArray"; - using value_array_t = std::array; - using pid_array_t = std::array; - -public: - JointPIDArray() = delete; - JointPIDArray(const PIDParameters &pid_parameters); - - void compute(const value_array_t &command_target, const value_array_t &state, - const std::chrono::nanoseconds &dt, value_array_t &command); - void compute(const value_array_t &command_target, const double *state, - const std::chrono::nanoseconds &dt, value_array_t &command); void log_info() const; protected: - PIDParameters pid_parameters_; /**< PID parameters for all joints.*/ - pid_array_t pid_controllers_; /**< PID controllers for each joint.*/ + bool initialized_{false}; /**< True if initialized.*/ + ExponentialFilter exponential_filter_; /**< Exponential filter applied to all joints.*/ }; } // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__FILTERS_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp index b4257a9e..d7b93a0b 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -4,67 +4,88 @@ #include #include #include +#include #include +#include #include "eigen3/Eigen/Core" -#include "kdl/chain.hpp" -#include "kdl/chainfksolverpos_recursive.hpp" -#include "kdl/chainjnttojacsolver.hpp" -#include "kdl/tree.hpp" -#include "kdl_parser/kdl_parser.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" #include "friLBRState.h" #include "lbr_fri_idl/msg/lbr_state.hpp" +#include "lbr_fri_ros2/kinematics.hpp" #include "lbr_fri_ros2/pinv.hpp" +#include "lbr_fri_ros2/types.hpp" +#include "lbr_fri_ros2/worker.hpp" namespace lbr_fri_ros2 { -class FTEstimator { +class FTEstimatorImpl { + /** + * @brief A class to estimate force-torques from external joint torque readings. Note that only + * forces beyond a specified threshold are returned. The specified threshold is removed from the + * estimated force-torque. + * + */ protected: - static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::FTEstimator"; - using jnt_pos_array_t = lbr_fri_idl::msg::LBRState::_measured_joint_position_type; - using const_jnt_pos_array_t_ref = const jnt_pos_array_t &; - using ext_tau_array_t = lbr_fri_idl::msg::LBRState::_external_torque_type; - using const_ext_tau_array_t_ref = const ext_tau_array_t &; + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::FTEstimatorImpl"; public: - static constexpr uint8_t CARTESIAN_DOF = 6; - using cart_array_t = std::array; - using cart_array_t_ref = cart_array_t &; - using const_cart_array_t_ref = const cart_array_t &; - - FTEstimator(const std::string &robot_description, const std::string &chain_root = "link_0", - const std::string &chain_tip = "link_ee", - const_cart_array_t_ref f_ext_th = {2., 2., 2., 0.5, 0.5, 0.5}); - void compute(const_jnt_pos_array_t_ref measured_joint_position, - const_ext_tau_array_t_ref external_torque, cart_array_t_ref f_ext, - const double &damping = 0.2); + FTEstimatorImpl(const std::string &robot_description, + const std::string &chain_root = "lbr_link_0", + const std::string &chain_tip = "lbr_link_ee", + const_cart_array_t_ref f_ext_th = {2., 2., 2., 0.5, 0.5, 0.5}, + const double &damping = 0.2); + void compute(); void reset(); + inline void get_f_ext(cart_array_t_ref f_ext) const { + Eigen::Map>(f_ext.data()) = f_ext_; + } + inline void get_f_ext_tf(cart_array_t_ref f_ext) const { + Eigen::Map>(f_ext.data()) = f_ext_tf_; + } + inline void set_tau_ext(const_jnt_array_t_ref tau_ext) { + tau_ext_ = Eigen::Map>(tau_ext.data()); + } + inline void set_q(const_jnt_array_t_ref q) { q_ = q; } + protected: // force threshold cart_array_t f_ext_th_; - KDL::Tree tree_; - KDL::Chain chain_; - - // solvers - std::unique_ptr jacobian_solver_; - std::unique_ptr fk_solver_; + // damping for pseudo-inverse of Jacobian + double damping_; - // robot state - KDL::JntArray q_; + // joint positions and external joint torques + jnt_array_t q_; - // forward kinematics - KDL::Frame chain_tip_frame_; + // kinematics + std::unique_ptr kinematics_ptr_; // force estimation - KDL::Jacobian jacobian_; - Eigen::Matrix jacobian_inv_; - Eigen::Matrix tau_ext_; - Eigen::Matrix f_ext_; + Eigen::Matrix jacobian_inv_; + Eigen::Matrix tau_ext_; + Eigen::Matrix f_ext_raw_, f_ext_, f_ext_tf_; +}; + +class FTEstimator : public Worker { + /** + * @brief A simple class to run the FTEstimatorImpl asynchronously at a specified update rate. + * + */ +public: + FTEstimator(const std::shared_ptr ft_estimator, + const std::uint16_t &update_rate = 100); + + inline std::string LOGGER_NAME() const override { return "lbr_fri_ros2::FTEstimator"; }; + +protected: + void perform_work_() override; + + std::shared_ptr ft_estimator_impl_ptr_; + std::uint16_t update_rate_; }; } // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__FT_ESTIMATOR_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp index eab3b4a3..4aa3c582 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp @@ -10,31 +10,21 @@ #include "rclcpp/logging.hpp" #include "friClientVersion.h" -#include "friLBRClient.h" #include "lbr_fri_idl/msg/lbr_command.hpp" #include "lbr_fri_ros2/command_guard.hpp" #include "lbr_fri_ros2/filters.hpp" #include "lbr_fri_ros2/formatting.hpp" +#include "lbr_fri_ros2/types.hpp" namespace lbr_fri_ros2 { class BaseCommandInterface { protected: virtual std::string LOGGER_NAME() const = 0; - // ROS IDL types - using idl_command_t = lbr_fri_idl::msg::LBRCommand; - using const_idl_command_t_ref = const idl_command_t &; - using idl_state_t = lbr_fri_idl::msg::LBRState; - using const_idl_state_t_ref = const idl_state_t &; - - // FRI types - using fri_command_t = KUKA::FRI::LBRCommand; - using fri_command_t_ref = fri_command_t &; - public: BaseCommandInterface() = delete; - BaseCommandInterface(const PIDParameters &pid_parameters, + BaseCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); @@ -49,7 +39,7 @@ class BaseCommandInterface { protected: std::unique_ptr command_guard_; - JointPIDArray joint_position_pid_; + JointExponentialFilterArray joint_position_filter_; idl_command_t command_, command_target_; }; } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp index 24e8a324..399beed7 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp @@ -12,7 +12,7 @@ class PositionCommandInterface : public BaseCommandInterface { public: PositionCommandInterface() = delete; - PositionCommandInterface(const PIDParameters &pid_parameters, + PositionCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp index 1a943bd7..ca0a6068 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp @@ -1,46 +1,37 @@ #ifndef LBR_FRI_ROS2__INTERFACES__STATE_HPP_ #define LBR_FRI_ROS2__INTERFACES__STATE_HPP_ + #include +#include #include #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" #include "friClientVersion.h" -#include "friLBRClient.h" #include "lbr_fri_idl/msg/lbr_state.hpp" #include "lbr_fri_ros2/filters.hpp" +#include "lbr_fri_ros2/types.hpp" namespace lbr_fri_ros2 { struct StateInterfaceParameters { - double external_torque_cutoff_frequency; /*Hz*/ - double measured_torque_cutoff_frequency; /*Hz*/ + double external_torque_tau; /*seconds*/ + double measured_torque_tau; /*seconds*/ }; class StateInterface { protected: static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::StateInterface"; - // ROS IDL types - using idl_state_t = lbr_fri_idl::msg::LBRState; - using const_idl_state_t_ref = const idl_state_t &; - using idl_joint_pos_t = idl_state_t::_measured_joint_position_type; - using const_idl_joint_pos_t_ref = const idl_joint_pos_t &; - - // FRI types - using fri_state_t = KUKA::FRI::LBRState; - using const_fri_state_t_ref = const fri_state_t &; - using fri_session_state_t = KUKA::FRI::ESessionState; - public: StateInterface() = delete; - StateInterface(const StateInterfaceParameters &state_interface_parameters = {10.0, 10.0}); + StateInterface(const StateInterfaceParameters &state_interface_parameters = {0.04, 0.04}); inline const_idl_state_t_ref get_state() const { return state_; }; void set_state(const_fri_state_t_ref state); - void set_state_open_loop(const_fri_state_t_ref state, const_idl_joint_pos_t_ref joint_position); + void set_state_open_loop(const_fri_state_t_ref state, const_jnt_array_t_ref joint_position); inline void uninitialize() { state_initialized_ = false; } inline bool is_initialized() const { return state_initialized_; }; diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp index e4ec7874..70536bee 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp @@ -12,7 +12,7 @@ class TorqueCommandInterface : public BaseCommandInterface { public: TorqueCommandInterface() = delete; - TorqueCommandInterface(const PIDParameters &pid_parameters, + TorqueCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp index 0c846ee4..4e68fae0 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp @@ -12,7 +12,7 @@ class WrenchCommandInterface : public BaseCommandInterface { public: WrenchCommandInterface() = delete; - WrenchCommandInterface(const PIDParameters &pid_parameters, + WrenchCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant = "default"); diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/kinematics.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/kinematics.hpp new file mode 100644 index 00000000..4192d400 --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/kinematics.hpp @@ -0,0 +1,55 @@ +#ifndef LBR_FRI_ROS2__KINEMATICS_HPP_ +#define LBR_FRI_ROS2__KINEMATICS_HPP_ + +#include + +#include "eigen3/Eigen/Core" +#include "kdl/chain.hpp" +#include "kdl/chainfksolverpos_recursive.hpp" +#include "kdl/chainjnttojacsolver.hpp" +#include "kdl/tree.hpp" +#include "kdl_parser/kdl_parser.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" + +#include "friLBRState.h" + +#include "lbr_fri_idl/msg/lbr_state.hpp" +#include "lbr_fri_ros2/types.hpp" + +namespace lbr_fri_ros2 { +class Kinematics { +protected: + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::Kinematics"; + +public: + Kinematics(const std::string &robot_description, const std::string &chain_root = "lbr_link_0", + const std::string &chain_tip = "lbr_link_ee"); + + // internally computes the jacobian and return a reference to it + const KDL::Jacobian &compute_jacobian(const_jnt_array_t_ref q); + const KDL::Jacobian &compute_jacobian(const std::vector &q); + + // forward kinematics + const KDL::Frame &compute_fk(const_jnt_array_t_ref q); + const KDL::Frame &compute_fk(const std::vector &q); + +protected: + KDL::Tree tree_; + KDL::Chain chain_; + + // solvers + std::unique_ptr jacobian_solver_; + std::unique_ptr fk_solver_; + + // robot state + KDL::JntArray q_; + + // forward kinematics + KDL::Frame chain_tip_frame_; + + // jacobian + KDL::Jacobian jacobian_; +}; +} // namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__KINEMATICS_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp new file mode 100644 index 00000000..faddc9eb --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp @@ -0,0 +1,51 @@ +#ifndef LBR_FRI_ROS2__TYPES_HPP_ +#define LBR_FRI_ROS2__TYPES_HPP_ + +#include +#include +#include + +#include "friLBRClient.h" + +#include "lbr_fri_idl/msg/lbr_command.hpp" +#include "lbr_fri_idl/msg/lbr_state.hpp" + +namespace lbr_fri_ros2 { +// joint DoF alias +constexpr std::uint8_t N_JNTS = KUKA::FRI::LBRState::NUMBER_OF_JOINTS; + +// joint positions, velocities, accelerations, torques etc. +using jnt_array_t = std::array; +using jnt_array_t_ref = jnt_array_t &; +using const_jnt_array_t_ref = const jnt_array_t &; + +// joint names +using jnt_name_array_t = std::array; +using jnt_name_array_t_ref = jnt_name_array_t &; +using const_jnt_name_array_t_ref = const jnt_name_array_t &; + +// Cartesian DoF +constexpr std::uint8_t CARTESIAN_DOF = 6; + +// Cartesian positions, velocities, accelerations, wrenches etc. +using cart_array_t = std::array; +using cart_array_t_ref = cart_array_t &; +using const_cart_array_t_ref = const cart_array_t &; + +// FRI types +using fri_command_t = KUKA::FRI::LBRCommand; +using fri_command_t_ref = fri_command_t &; +using const_fri_command_t_ref = const fri_command_t &; +using fri_state_t = KUKA::FRI::LBRState; +using fri_state_t_ref = fri_state_t &; +using const_fri_state_t_ref = const fri_state_t &; + +// ROS IDL types +using idl_command_t = lbr_fri_idl::msg::LBRCommand; +using idl_command_t_ref = idl_command_t &; +using const_idl_command_t_ref = const idl_command_t &; +using idl_state_t = lbr_fri_idl::msg::LBRState; +using idl_state_t_ref = idl_state_t &; +using const_idl_state_t_ref = const idl_state_t &; +} // namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__TYPES_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp new file mode 100644 index 00000000..1eb42b8f --- /dev/null +++ b/lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp @@ -0,0 +1,31 @@ +#ifndef LBR_FRI_ROS2__WORKER_HPP_ +#define LBR_FRI_ROS2__WORKER_HPP_ + +#include +#include +#include + +#include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" +#include "realtime_tools/thread_priority.hpp" + +#include "lbr_fri_ros2/formatting.hpp" + +namespace lbr_fri_ros2 { +class Worker { +public: + Worker(); + ~Worker(); + + virtual void run_async(int rt_prio = 80); + void request_stop(); + inline virtual std::string LOGGER_NAME() const = 0; + +protected: + virtual void perform_work_() = 0; + + std::atomic_bool should_stop_, running_; + std::thread run_thread_; +}; +} // namespace lbr_fri_ros2 +#endif // LBR_FRI_ROS2__WORKER_HPP_ diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index 62379bc1..879bbbea 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -2,7 +2,7 @@ lbr_fri_ros2 - 2.1.2 + 2.2.0 The lbr_fri_ros2 package provides the Fast Robot Interface (FRI) integration into ROS 2. Robot states can be extracted and commanded. mhubii @@ -13,8 +13,8 @@ eigen - control_toolbox fri_client_sdk + geometry_msgs kdl_parser lbr_fri_idl orocos_kdl_vendor diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index 7642a814..9be75fe8 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -2,8 +2,7 @@ namespace lbr_fri_ros2 { App::App(const std::shared_ptr async_client_ptr) - : should_stop_(true), running_(false), async_client_ptr_(nullptr), connection_ptr_(nullptr), - app_ptr_(nullptr) { + : async_client_ptr_(nullptr), connection_ptr_(nullptr), app_ptr_(nullptr) { async_client_ptr_ = async_client_ptr; connection_ptr_ = std::make_unique(); app_ptr_ = std::make_unique(*connection_ptr_, *async_client_ptr_); @@ -16,115 +15,96 @@ App::~App() { bool App::open_udp_socket(const int &port_id, const char *const remote_host) { if (!connection_ptr_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "Connection not configured" << ColorScheme::ENDC); return false; } - RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::OKBLUE << "Opening UDP socket with port_id '" << ColorScheme::BOLD << port_id << "'" << ColorScheme::ENDC); if (!valid_port_(port_id)) { return false; } if (connection_ptr_->isOpen()) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket already open"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Socket already open"); return true; } if (!connection_ptr_->open(port_id, remote_host)) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "Failed to open socket" << ColorScheme::ENDC); return false; } - RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::OKGREEN << "Socket opened successfully" << ColorScheme::ENDC); return true; } bool App::close_udp_socket() { if (!connection_ptr_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "Connection not configured" << ColorScheme::ENDC); return false; } while (running_) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Waiting for run thread termination"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Waiting for run thread termination"); std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::OKBLUE << "Closing UDP socket" << ColorScheme::ENDC); if (!connection_ptr_->isOpen()) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket already closed"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Socket already closed"); return true; } connection_ptr_->close(); - RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::OKGREEN << "Socket closed successfully" << ColorScheme::ENDC); return true; } void App::run_async(int rt_prio) { if (!async_client_ptr_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "AsyncClient not configured" << ColorScheme::ENDC); return; } if (!connection_ptr_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "Connection not configured" << ColorScheme::ENDC); return; } if (!connection_ptr_->isOpen()) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "Connection not open" << ColorScheme::ENDC); return; } if (!app_ptr_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "App not configured" << ColorScheme::ENDC); return; } - if (running_) { - RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::WARNING << "App already running" << ColorScheme::ENDC); - return; - } - run_thread_ = std::thread([&]() { - if (realtime_tools::has_realtime_kernel()) { - if (!realtime_tools::configure_sched_fifo(rt_prio)) { - RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME), - ColorScheme::WARNING << "Failed to set FIFO realtime scheduling policy" - << ColorScheme::ENDC); - } - } else { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Realtime kernel recommended but not required"); - } - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Starting run thread"); - should_stop_ = false; - bool success = true; - while (rclcpp::ok() && success && !should_stop_) { - success = app_ptr_->step(); // stuck if never connected - running_ = true; - if (async_client_ptr_->robotState().getSessionState() == KUKA::FRI::ESessionState::IDLE) { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR in session state idle, exiting"); - break; - } - } - async_client_ptr_->get_state_interface()->uninitialize(); - running_ = false; - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Exiting run thread"); - }); + // call base class run_async post checks + Worker::run_async(rt_prio); run_thread_.detach(); } -void App::request_stop() { - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Requesting run thread stop"); - should_stop_ = true; +void App::perform_work_() { + bool success = true; + while (rclcpp::ok() && success && !should_stop_) { + success = app_ptr_->step(); // stuck if never connected, we thus detach the run_thread_ as join + // may never return + running_ = true; + if (async_client_ptr_->robotState().getSessionState() == KUKA::FRI::ESessionState::IDLE) { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "LBR in session state idle, exiting"); + break; + } + } + async_client_ptr_->get_state_interface()->uninitialize(); } bool App::valid_port_(const int &port_id) { if (port_id < 30200 || port_id > 30209) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()), ColorScheme::ERROR << "Expected port_id in [30200, 30209], got '" << ColorScheme::BOLD << port_id << "'" << ColorScheme::ENDC); diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 14ad96bf..7d11414c 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -2,7 +2,7 @@ namespace lbr_fri_ros2 { AsyncClient::AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mode, - const PIDParameters &pid_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant, const StateInterfaceParameters &state_interface_parameters, @@ -26,16 +26,16 @@ AsyncClient::AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mod #endif { command_interface_ptr_ = std::make_shared( - pid_parameters, command_guard_parameters, command_guard_variant); + joint_position_tau, command_guard_parameters, command_guard_variant); break; } case KUKA::FRI::EClientCommandMode::TORQUE: command_interface_ptr_ = std::make_shared( - pid_parameters, command_guard_parameters, command_guard_variant); + joint_position_tau, command_guard_parameters, command_guard_variant); break; case KUKA::FRI::EClientCommandMode::WRENCH: command_interface_ptr_ = std::make_shared( - pid_parameters, command_guard_parameters, command_guard_variant); + joint_position_tau, command_guard_parameters, command_guard_variant); break; default: std::string err = "Unsupported client command mode."; diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp index 92d06e5b..c7bff823 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -2,7 +2,7 @@ namespace lbr_fri_ros2 { CommandGuard::CommandGuard(const CommandGuardParameters &command_guard_parameters) - : parameters_(command_guard_parameters), prev_measured_joint_position_init_(false){}; + : parameters_(command_guard_parameters), prev_measured_joint_position_init_(false) {}; bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command, const_idl_state_t_ref lbr_state) { diff --git a/lbr_fri_ros2/src/control.cpp b/lbr_fri_ros2/src/control.cpp new file mode 100644 index 00000000..30e61cbb --- /dev/null +++ b/lbr_fri_ros2/src/control.cpp @@ -0,0 +1,138 @@ +#include "lbr_fri_ros2/control.hpp" + +namespace lbr_fri_ros2 { +InvJacCtrlImpl::InvJacCtrlImpl(const std::string &robot_description, + const InvJacCtrlParameters ¶meters) + : parameters_(parameters) { + kinematics_ptr_ = std::make_unique(robot_description, parameters_.chain_root, + parameters_.chain_tip); + set_all_zero_(); + q_gains_ = Eigen::Map>(parameters_.joint_gains.data()); + x_gains_ = + Eigen::Map>(parameters_.cartesian_gains.data()); +} + +void InvJacCtrlImpl::compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target, + const_jnt_array_t_ref q, jnt_array_t_ref dq) { + // twist to Eigen + twist_target_[0] = twist_target->linear.x; + twist_target_[1] = twist_target->linear.y; + twist_target_[2] = twist_target->linear.z; + twist_target_[3] = twist_target->angular.x; + twist_target_[4] = twist_target->angular.y; + twist_target_[5] = twist_target->angular.z; + + // compute + compute_impl_(q, dq); +} + +void InvJacCtrlImpl::compute(const_cart_array_t_ref twist_target, const_jnt_array_t_ref q, + jnt_array_t_ref dq) { + // twist to Eigen + twist_target_[0] = twist_target[0]; + twist_target_[1] = twist_target[1]; + twist_target_[2] = twist_target[2]; + twist_target_[3] = twist_target[3]; + twist_target_[4] = twist_target[4]; + twist_target_[5] = twist_target[5]; + + // compute + compute_impl_(q, dq); +} + +void InvJacCtrlImpl::compute(const Eigen::Matrix &twist_target, + const_jnt_array_t_ref q, jnt_array_t_ref dq) { + twist_target_ = twist_target; + + // compute + compute_impl_(q, dq); +} + +void InvJacCtrlImpl::log_info() const { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Chain root: %s", + parameters_.chain_root.c_str()); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Chain tip: %s", parameters_.chain_tip.c_str()); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Twist in tip frame: %s", + parameters_.twist_in_tip_frame ? "true" : "false"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Damping: %.3f", parameters_.damping); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Max linear velocity: %.3f", + parameters_.max_linear_velocity); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Max angular velocity: %.3f", + parameters_.max_angular_velocity); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), + "* Joint gains: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", + parameters_.joint_gains[0], parameters_.joint_gains[1], parameters_.joint_gains[2], + parameters_.joint_gains[3], parameters_.joint_gains[4], parameters_.joint_gains[5], + parameters_.joint_gains[6]); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), + "* Cartesian gains: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", + parameters_.cartesian_gains[0], parameters_.cartesian_gains[1], + parameters_.cartesian_gains[2], parameters_.cartesian_gains[3], + parameters_.cartesian_gains[4], parameters_.cartesian_gains[5]); +} + +void AdmittanceImpl::compute(const Eigen::Matrix &f_ext, + const Eigen::Matrix &x, + const Eigen::Matrix &dx, + Eigen::Matrix &ddx) { + if ((m_.array() < 0.).any()) { + throw std::runtime_error("Mass must be positive and greater zero."); + } + ddx = (f_ext - b_.asDiagonal() * dx - k_.asDiagonal() * x).array() / m_.array(); +} + +void AdmittanceImpl::log_info() const { + { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* Mass: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", + parameters_.m[0], parameters_.m[1], parameters_.m[2], parameters_.m[3], + parameters_.m[4], parameters_.m[5]); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), + "* Damping: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", parameters_.b[0], + parameters_.b[1], parameters_.b[2], parameters_.b[3], parameters_.b[4], + parameters_.b[5]); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), + "* Stiffness: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", parameters_.k[0], + parameters_.k[1], parameters_.k[2], parameters_.k[3], parameters_.k[4], + parameters_.k[5]); + } +} + +void InvJacCtrlImpl::compute_impl_(const_jnt_array_t_ref q, jnt_array_t_ref dq) { + // clip velocity + twist_target_.head(3).unaryExpr([&](double v) { + return std::clamp(v, -parameters_.max_linear_velocity, parameters_.max_linear_velocity); + }); + twist_target_.tail(3).unaryExpr([&](double v) { + return std::clamp(v, -parameters_.max_angular_velocity, parameters_.max_angular_velocity); + }); + + // if desired, transform to tip frame + if (parameters_.twist_in_tip_frame) { + auto chain_tip_frame = kinematics_ptr_->compute_fk(q); + twist_target_.topRows(3) = + Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.topRows(3); + twist_target_.bottomRows(3) = + Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.bottomRows(3); + } + + twist_target_ = x_gains_.asDiagonal() * twist_target_; + + // compute jacobian + auto jacobian = kinematics_ptr_->compute_jacobian(q); + jacobian_inv_ = pinv(jacobian.data, parameters_.damping); + + // compute target joint veloctiy and map it to dq + Eigen::Map>(dq.data()) = + q_gains_.asDiagonal() * jacobian_inv_ * twist_target_; +} + +void InvJacCtrlImpl::set_all_zero_() { + std::fill(q_.begin(), q_.end(), 0.0); + jacobian_inv_.setZero(); + twist_target_.setZero(); + q_gains_.setZero(); + x_gains_.setZero(); +} +} // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/filters.cpp b/lbr_fri_ros2/src/filters.cpp index 4047e59a..2bf0ed2c 100644 --- a/lbr_fri_ros2/src/filters.cpp +++ b/lbr_fri_ros2/src/filters.cpp @@ -1,83 +1,64 @@ #include "lbr_fri_ros2/filters.hpp" namespace lbr_fri_ros2 { -ExponentialFilter::ExponentialFilter() : ExponentialFilter::ExponentialFilter(0, 0.0) {} +ExponentialFilter::ExponentialFilter() + : tau_(std::numeric_limits::quiet_NaN()), + sample_time_(std::numeric_limits::quiet_NaN()), + alpha_(std::numeric_limits::quiet_NaN()) {} -ExponentialFilter::ExponentialFilter(const double &cutoff_frequency, const double &sample_time) { - set_cutoff_frequency(cutoff_frequency, sample_time); +ExponentialFilter::ExponentialFilter(const double &tau) : tau_(tau) {} + +void ExponentialFilter::initialize(const double &sample_time) { + if (std::isnan(tau_)) { + throw std::runtime_error("Time constant must be set before initializing."); + } + return initialize(tau_, sample_time); } -void ExponentialFilter::set_cutoff_frequency(const double &cutoff_frequency, - const double &sample_time) { - cutoff_frequency_ = cutoff_frequency; - if (cutoff_frequency_ > (1. / sample_time)) { - cutoff_frequency_ = (1. / sample_time); +void ExponentialFilter::initialize(const double &tau, const double &sample_time) { + if (tau <= 0.0) { + throw std::runtime_error("Time constant must be positive and greater zero."); } - sample_time_ = sample_time; - alpha_ = compute_alpha_(cutoff_frequency, sample_time); - if (!validate_alpha_(alpha_)) { + if (sample_time < 0.0) { + throw std::runtime_error("Sample time must be positive."); + } + double alpha = 1.0 - std::exp(-sample_time / tau); + if (!validate_alpha_(alpha)) { throw std::runtime_error("Alpha is not within [0, 1]"); } -} - -double ExponentialFilter::compute_alpha_(const double &cutoff_frequency, - const double &sample_time) { - double omega_3db = 2.0 * M_PI * sample_time * cutoff_frequency; - return std::cos(omega_3db) - 1 + - std::sqrt(std::pow(std::cos(omega_3db), 2) - 4 * std::cos(omega_3db) + 3); + tau_ = tau; + sample_time_ = sample_time; + alpha_ = alpha; } bool ExponentialFilter::validate_alpha_(const double &alpha) { return alpha <= 1. && alpha >= 0.; } -void JointExponentialFilterArray::compute(const double *const current, value_array_t &previous) { - std::for_each(current, current + KUKA::FRI::LBRState::NUMBER_OF_JOINTS, - [&, i = 0](const auto ¤t_i) mutable { - previous[i] = exponential_filter_.compute(current_i, previous[i]); - ++i; - }); -} +JointExponentialFilterArray::JointExponentialFilterArray(const double &tau) + : exponential_filter_(tau) {} -void JointExponentialFilterArray::initialize(const double &cutoff_frequency, - const double &sample_time) { - exponential_filter_.set_cutoff_frequency(cutoff_frequency, sample_time); - initialized_ = true; +void JointExponentialFilterArray::compute(const double *const current, jnt_array_t_ref previous) { + std::for_each(current, current + N_JNTS, [&, i = 0](const auto ¤t_i) mutable { + previous[i] = exponential_filter_.compute(current_i, previous[i]); + ++i; + }); } -JointPIDArray::JointPIDArray(const PIDParameters &pid_parameters) - : pid_parameters_(pid_parameters) // keep local copy of parameters since - // controller_toolbox::Pid::getGains is not const correct - // (i.e. can't be called in this->log_info) -{ - std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) { - pid.initPid(pid_parameters_.p, pid_parameters_.i, pid_parameters_.d, pid_parameters_.i_max, - pid_parameters_.i_min, pid_parameters_.antiwindup); - }); +void JointExponentialFilterArray::compute(const_jnt_array_t_ref current, jnt_array_t_ref previous) { + compute(current.data(), previous); } -void JointPIDArray::compute(const value_array_t &command_target, const value_array_t &state, - const std::chrono::nanoseconds &dt, value_array_t &command) { - std::for_each(command.begin(), command.end(), [&, i = 0](double &command_i) mutable { - command_i += pid_controllers_[i].computeCommand(command_target[i] - state[i], dt.count()); - ++i; - }); +void JointExponentialFilterArray::initialize(const double &sample_time) { + exponential_filter_.initialize(sample_time); + initialized_ = true; } -void JointPIDArray::compute(const value_array_t &command_target, const double *state, - const std::chrono::nanoseconds &dt, value_array_t &command) { - std::for_each(command.begin(), command.end(), [&, i = 0](double &command_i) mutable { - command_i += pid_controllers_[i].computeCommand(command_target[i] - state[i], dt.count()); - ++i; - }); +void JointExponentialFilterArray::initialize(const double &tau, const double &sample_time) { + exponential_filter_.initialize(tau, sample_time); + initialized_ = true; } -void JointPIDArray::log_info() const { +void JointExponentialFilterArray::log_info() const { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* p: %.1f", pid_parameters_.p); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i: %.1f", pid_parameters_.i); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* d: %.1f", pid_parameters_.d); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i_max: %.1f", pid_parameters_.i_max); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i_min: %.1f", pid_parameters_.i_min); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* antiwindup: %s", - pid_parameters_.antiwindup ? "true" : "false"); -}; + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* tau: %.5f s", exponential_filter_.get_tau()); +} } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp index 8ac0981c..e846ba6f 100644 --- a/lbr_fri_ros2/src/ft_estimator.cpp +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -1,59 +1,54 @@ #include "lbr_fri_ros2/ft_estimator.hpp" namespace lbr_fri_ros2 { -FTEstimator::FTEstimator(const std::string &robot_description, const std::string &chain_root, - const std::string &chain_tip, const_cart_array_t_ref f_ext_th) - : f_ext_th_(f_ext_th) { - if (!kdl_parser::treeFromString(robot_description, tree_)) { - std::string err = "Failed to construct kdl tree from robot description."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); - throw std::runtime_error(err); - } - if (!tree_.getChain(chain_root, chain_tip, chain_)) { - std::string err = "Failed to construct kdl chain from robot description."; - RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); - throw std::runtime_error(err); - } - jacobian_solver_ = std::make_unique(chain_); - fk_solver_ = std::make_unique(chain_); - jacobian_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - q_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - +FTEstimatorImpl::FTEstimatorImpl(const std::string &robot_description, + const std::string &chain_root, const std::string &chain_tip, + const_cart_array_t_ref f_ext_th, const double &damping) + : f_ext_th_(f_ext_th), damping_(damping) { + kinematics_ptr_ = std::make_unique(robot_description, chain_root, chain_tip); reset(); } -void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position, - const_ext_tau_array_t_ref external_torque, cart_array_t_ref f_ext, - const double &damping) { - q_.data = Eigen::Map>( - measured_joint_position.data()); - tau_ext_ = Eigen::Map>( - external_torque.data()); - jacobian_solver_->JntToJac(q_, jacobian_); - jacobian_inv_ = pinv(jacobian_.data, damping); - f_ext_ = jacobian_inv_.transpose() * tau_ext_; +void FTEstimatorImpl::compute() { + auto jacobian = kinematics_ptr_->compute_jacobian(q_); + jacobian_inv_ = pinv(jacobian.data, damping_); + f_ext_raw_ = jacobian_inv_.transpose() * tau_ext_; + int i = -1; + f_ext_ = f_ext_raw_.unaryExpr([&](double v) { + ++i; + if (std::abs(v) < f_ext_th_[i]) { + return 0.; + } else { + return std::copysign(1., v) * (std::abs(v) - f_ext_th_[i]); + } + }); // rotate into chain tip frame - fk_solver_->JntToCart(q_, chain_tip_frame_); - f_ext_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame_.M.data) * f_ext_.topRows(3); - f_ext_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame_.M.data) * f_ext_.bottomRows(3); - - Eigen::Map>(f_ext.data()) = f_ext_; - - // threshold force-torque - std::transform(f_ext.begin(), f_ext.end(), f_ext_th_.begin(), f_ext.begin(), - [](const double &f_ext_i, const double &f_ext_th_i) { - if (std::abs(f_ext_i) < f_ext_th_i) { - return 0.; - } else { - return std::copysign(1., f_ext_i) * (std::abs(f_ext_i) - f_ext_th_i); - } - }); + auto chain_tip_frame = kinematics_ptr_->compute_fk(q_); + f_ext_tf_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.topRows(3); + f_ext_tf_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.bottomRows(3); } -void FTEstimator::reset() { - q_.data.setZero(); +void FTEstimatorImpl::reset() { + std::for_each(q_.begin(), q_.end(), [](double &q_i) { q_i = 0.; }); tau_ext_.setZero(); + f_ext_raw_.setZero(); f_ext_.setZero(); + f_ext_tf_.setZero(); + jacobian_inv_.setZero(); } + +FTEstimator::FTEstimator(const std::shared_ptr ft_estimator_impl_ptr, + const std::uint16_t &update_rate) + : ft_estimator_impl_ptr_(ft_estimator_impl_ptr), update_rate_(update_rate) {} + +void FTEstimator::perform_work_() { + running_ = true; + while (rclcpp::ok() && !should_stop_) { + auto start = std::chrono::high_resolution_clock::now(); + ft_estimator_impl_ptr_->compute(); + std::this_thread::sleep_until(start + std::chrono::nanoseconds(static_cast( + 1.e9 / static_cast(update_rate_)))); + } +}; } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/base_command.cpp b/lbr_fri_ros2/src/interfaces/base_command.cpp index f53bddeb..e43111ea 100644 --- a/lbr_fri_ros2/src/interfaces/base_command.cpp +++ b/lbr_fri_ros2/src/interfaces/base_command.cpp @@ -2,10 +2,10 @@ namespace lbr_fri_ros2 { -BaseCommandInterface::BaseCommandInterface(const PIDParameters &pid_parameters, +BaseCommandInterface::BaseCommandInterface(const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : joint_position_pid_(pid_parameters) { + : joint_position_filter_(joint_position_tau) { command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant); }; @@ -18,6 +18,6 @@ void BaseCommandInterface::init_command(const_idl_state_t_ref state) { void BaseCommandInterface::log_info() const { command_guard_->log_info(); - joint_position_pid_.log_info(); + joint_position_filter_.log_info(); } } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp index b80530f3..95533981 100644 --- a/lbr_fri_ros2/src/interfaces/position_command.cpp +++ b/lbr_fri_ros2/src/interfaces/position_command.cpp @@ -2,9 +2,9 @@ namespace lbr_fri_ros2 { PositionCommandInterface::PositionCommandInterface( - const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : BaseCommandInterface(pid_parameters, command_guard_parameters, command_guard_variant) {} + : BaseCommandInterface(joint_position_tau, command_guard_parameters, command_guard_variant) {} void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) { @@ -40,11 +40,11 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command throw std::runtime_error(err); } - // PID - joint_position_pid_.compute( - command_target_.joint_position, state.measured_joint_position, - std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), - command_.joint_position); + // exponential smooth + if (!joint_position_filter_.is_initialized()) { + joint_position_filter_.initialize(state.sample_time); + } + joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); // validate if (!command_guard_->is_valid_command(command_, state)) { diff --git a/lbr_fri_ros2/src/interfaces/state.cpp b/lbr_fri_ros2/src/interfaces/state.cpp index 7e49c9bd..d34b457e 100644 --- a/lbr_fri_ros2/src/interfaces/state.cpp +++ b/lbr_fri_ros2/src/interfaces/state.cpp @@ -15,15 +15,13 @@ void StateInterface::set_state(const_fri_state_t_ref state) { state_.connection_quality = state.getConnectionQuality(); state_.control_mode = state.getControlMode(); state_.drive_state = state.getDriveState(); - external_torque_filter_.compute(state.getExternalTorque(), state_.external_torque); - if (state.getSessionState() == fri_session_state_t::COMMANDING_WAIT || - state.getSessionState() == fri_session_state_t::COMMANDING_ACTIVE) { + if (state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_WAIT || + state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { std::memcpy(state_.ipo_joint_position.data(), state.getIpoJointPosition(), sizeof(double) * fri_state_t::NUMBER_OF_JOINTS); } std::memcpy(state_.measured_joint_position.data(), state.getMeasuredJointPosition(), sizeof(double) * fri_state_t::NUMBER_OF_JOINTS); - measured_torque_filter_.compute(state.getMeasuredTorque(), state_.measured_torque); state_.operation_mode = state.getOperationMode(); state_.overlay_type = state.getOverlayType(); state_.safety_state = state.getSafetyState(); @@ -34,14 +32,19 @@ void StateInterface::set_state(const_fri_state_t_ref state) { state_.tracking_performance = state.getTrackingPerformance(); if (!external_torque_filter_.is_initialized() || !measured_torque_filter_.is_initialized()) { - // initialize once state_ is available + // initialize state_.sample_time is available init_filters_(); } + + // only compute after state_.sample_time is available + external_torque_filter_.compute(state.getExternalTorque(), state_.external_torque); + measured_torque_filter_.compute(state.getMeasuredTorque(), state_.measured_torque); + state_initialized_ = true; }; void StateInterface::set_state_open_loop(const_fri_state_t_ref state, - const_idl_joint_pos_t_ref joint_position) { + const_jnt_array_t_ref joint_position) { state_.client_command_mode = state.getClientCommandMode(); #if FRI_CLIENT_VERSION_MAJOR == 1 std::memcpy(state_.commanded_joint_position.data(), state.getCommandedJointPosition(), @@ -52,15 +55,13 @@ void StateInterface::set_state_open_loop(const_fri_state_t_ref state, state_.connection_quality = state.getConnectionQuality(); state_.control_mode = state.getControlMode(); state_.drive_state = state.getDriveState(); - external_torque_filter_.compute(state.getExternalTorque(), state_.external_torque); - if (state.getSessionState() == fri_session_state_t::COMMANDING_WAIT || - state.getSessionState() == fri_session_state_t::COMMANDING_ACTIVE) { + if (state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_WAIT || + state.getSessionState() == KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { std::memcpy(state_.ipo_joint_position.data(), state.getIpoJointPosition(), sizeof(double) * fri_state_t::NUMBER_OF_JOINTS); } std::memcpy(state_.measured_joint_position.data(), joint_position.data(), sizeof(double) * fri_state_t::NUMBER_OF_JOINTS); - measured_torque_filter_.compute(state.getMeasuredTorque(), state_.measured_torque); state_.operation_mode = state.getOperationMode(); state_.overlay_type = state.getOverlayType(); state_.safety_state = state.getSafetyState(); @@ -71,24 +72,27 @@ void StateInterface::set_state_open_loop(const_fri_state_t_ref state, state_.tracking_performance = state.getTrackingPerformance(); if (!external_torque_filter_.is_initialized() || !measured_torque_filter_.is_initialized()) { - // initialize once state_ is available + // initialize state_.sample_time is available init_filters_(); } + + // only compute after state_.sample_time is available + external_torque_filter_.compute(state.getExternalTorque(), state_.external_torque); + measured_torque_filter_.compute(state.getMeasuredTorque(), state_.measured_torque); + state_initialized_ = true; } void StateInterface::init_filters_() { - external_torque_filter_.initialize(parameters_.external_torque_cutoff_frequency, - state_.sample_time); - measured_torque_filter_.initialize(parameters_.measured_torque_cutoff_frequency, - state_.sample_time); + external_torque_filter_.initialize(parameters_.external_torque_tau, state_.sample_time); + measured_torque_filter_.initialize(parameters_.measured_torque_tau, state_.sample_time); } void StateInterface::log_info() const { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* external_torque_cutoff_frequency: %.1f Hz", - parameters_.external_torque_cutoff_frequency); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* measured_torque_cutoff_frequency: %.1f Hz", - parameters_.measured_torque_cutoff_frequency); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* external_torque_tau: %.5f s", + parameters_.external_torque_tau); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* measured_torque_tau: %.5f s", + parameters_.measured_torque_tau); } } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp index b658b266..54f53a2b 100644 --- a/lbr_fri_ros2/src/interfaces/torque_command.cpp +++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp @@ -2,9 +2,9 @@ namespace lbr_fri_ros2 { TorqueCommandInterface::TorqueCommandInterface( - const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : BaseCommandInterface(pid_parameters, command_guard_parameters, command_guard_variant) {} + : BaseCommandInterface(joint_position_tau, command_guard_parameters, command_guard_variant) {} void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) { @@ -30,12 +30,11 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, throw std::runtime_error(err); } - // PID - joint_position_pid_.compute( - command_target_.joint_position, state.measured_joint_position, - std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), - command_.joint_position); - command_.torque = command_target_.torque; + // exponential smooth + if (!joint_position_filter_.is_initialized()) { + joint_position_filter_.initialize(state.sample_time); + } + joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); // validate if (!command_guard_->is_valid_command(command_, state)) { diff --git a/lbr_fri_ros2/src/interfaces/wrench_command.cpp b/lbr_fri_ros2/src/interfaces/wrench_command.cpp index 9dd9618a..1a1c0ab7 100644 --- a/lbr_fri_ros2/src/interfaces/wrench_command.cpp +++ b/lbr_fri_ros2/src/interfaces/wrench_command.cpp @@ -2,9 +2,9 @@ namespace lbr_fri_ros2 { WrenchCommandInterface::WrenchCommandInterface( - const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, + const double &joint_position_tau, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : BaseCommandInterface(pid_parameters, command_guard_parameters, command_guard_variant) {} + : BaseCommandInterface(joint_position_tau, command_guard_parameters, command_guard_variant) {} void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) { @@ -28,12 +28,11 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, throw std::runtime_error(err); } - // PID - joint_position_pid_.compute( - command_target_.joint_position, state.measured_joint_position, - std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), - command_.joint_position); - command_.wrench = command_target_.wrench; + // exponential smooth + if (!joint_position_filter_.is_initialized()) { + joint_position_filter_.initialize(state.sample_time); + } + joint_position_filter_.compute(command_target_.joint_position, command_.joint_position); // validate if (!command_guard_->is_valid_command(command_, state)) { diff --git a/lbr_fri_ros2/src/kinematics.cpp b/lbr_fri_ros2/src/kinematics.cpp new file mode 100644 index 00000000..6cc56929 --- /dev/null +++ b/lbr_fri_ros2/src/kinematics.cpp @@ -0,0 +1,61 @@ +#include "lbr_fri_ros2/kinematics.hpp" + +namespace lbr_fri_ros2 { +Kinematics::Kinematics(const std::string &robot_description, const std::string &chain_root, + const std::string &chain_tip) { + if (!kdl_parser::treeFromString(robot_description, tree_)) { + std::string err = "Failed to construct kdl tree from robot description."; + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); + throw std::runtime_error(err); + } + if (!tree_.getChain(chain_root, chain_tip, chain_)) { + std::string err = "Failed to construct kdl chain from robot description."; + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); + throw std::runtime_error(err); + } + if (chain_.getNrOfJoints() != N_JNTS) { + std::string err = "Invalid number of joints in chain."; + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); + throw std::runtime_error(err); + } + jacobian_solver_ = std::make_unique(chain_); + fk_solver_ = std::make_unique(chain_); + jacobian_.resize(N_JNTS); + q_.resize(N_JNTS); + q_.data.setZero(); +} + +const KDL::Jacobian &Kinematics::compute_jacobian(const_jnt_array_t_ref q) { + q_.data = Eigen::Map>(q.data()); + jacobian_solver_->JntToJac(q_, jacobian_); + return jacobian_; +} + +const KDL::Jacobian &Kinematics::compute_jacobian(const std::vector &q) { + if (q.size() != N_JNTS) { + std::string err = "Invalid number of joint positions."; + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); + throw std::runtime_error(err); + } + q_.data = Eigen::Map>(q.data()); + jacobian_solver_->JntToJac(q_, jacobian_); + return jacobian_; +} + +const KDL::Frame &Kinematics::compute_fk(const_jnt_array_t_ref q) { + q_.data = Eigen::Map>(q.data()); + fk_solver_->JntToCart(q_, chain_tip_frame_); + return chain_tip_frame_; +} + +const KDL::Frame &Kinematics::compute_fk(const std::vector &q) { + if (q.size() != N_JNTS) { + std::string err = "Invalid number of joint positions."; + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); + throw std::runtime_error(err); + } + q_.data = Eigen::Map>(q.data()); + fk_solver_->JntToCart(q_, chain_tip_frame_); + return chain_tip_frame_; +} +} // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/worker.cpp b/lbr_fri_ros2/src/worker.cpp new file mode 100644 index 00000000..b42f55a0 --- /dev/null +++ b/lbr_fri_ros2/src/worker.cpp @@ -0,0 +1,53 @@ +#include "lbr_fri_ros2/worker.hpp" + +namespace lbr_fri_ros2 { +Worker::Worker() : should_stop_(true), running_(false) {} + +Worker::~Worker() { + this->request_stop(); + if (run_thread_.joinable()) { + run_thread_.join(); + } +} + +void Worker::run_async(int rt_prio) { + if (running_) { + RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME()), + ColorScheme::WARNING << "Worker already running" << ColorScheme::ENDC); + return; + } + run_thread_ = std::thread([this, rt_prio]() { + if (!realtime_tools::configure_sched_fifo(rt_prio)) { + RCLCPP_WARN_STREAM(rclcpp::get_logger(LOGGER_NAME()), + ColorScheme::WARNING + << "Failed to set FIFO realtime scheduling policy. Refer to " + "[https://control.ros.org/master/doc/ros2_control/" + "controller_manager/doc/userdoc.html]." + << ColorScheme::ENDC); + } else { + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), + ColorScheme::OKGREEN + << "Realtime scheduling policy set to FIFO with priority '" << rt_prio + << "'" << ColorScheme::ENDC); + } + + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Starting run thread"); + should_stop_ = false; + + // perform work in child-class + this->perform_work_(); + // perform work end + + running_ = false; + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Exiting run thread"); + }); +} + +void Worker::request_stop() { + if (!running_) { + return; + } + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Requesting run thread stop"); + should_stop_ = true; +} +} // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/test/test_command_interfaces.cpp b/lbr_fri_ros2/test/test_command_interfaces.cpp index 127fd2ee..d662643a 100644 --- a/lbr_fri_ros2/test/test_command_interfaces.cpp +++ b/lbr_fri_ros2/test/test_command_interfaces.cpp @@ -18,7 +18,6 @@ class TestCommandInterfaces : public ::testing::Test { public: TestCommandInterfaces() : random_engine_(std::random_device{}()) { - pid_params_ = lbr_fri_ros2::PIDParameters(); cmd_guard_params_ = lbr_fri_ros2::CommandGuardParameters(); state_interface_params_ = lbr_fri_ros2::StateInterfaceParameters(); @@ -40,17 +39,17 @@ class TestCommandInterfaces : public ::testing::Test { case KUKA::FRI::EClientCommandMode::JOINT_POSITION: #endif { - command_interface_ = - std::make_shared(pid_params_, cmd_guard_params_); + command_interface_ = std::make_shared( + joint_position_tau_, cmd_guard_params_); break; } case KUKA::FRI::EClientCommandMode::TORQUE: - command_interface_ = - std::make_shared(pid_params_, cmd_guard_params_); + command_interface_ = std::make_shared( + joint_position_tau_, cmd_guard_params_); break; case KUKA::FRI::EClientCommandMode::WRENCH: - command_interface_ = - std::make_shared(pid_params_, cmd_guard_params_); + command_interface_ = std::make_shared( + joint_position_tau_, cmd_guard_params_); break; default: throw std::runtime_error("Unsupported client command mode."); @@ -129,7 +128,7 @@ class TestCommandInterfaces : public ::testing::Test { std::default_random_engine random_engine_; std::uniform_real_distribution uniform_real_dist_{-1.0, 1.0}; - lbr_fri_ros2::PIDParameters pid_params_; + double joint_position_tau_{0.04}; lbr_fri_ros2::CommandGuardParameters cmd_guard_params_; lbr_fri_ros2::StateInterfaceParameters state_interface_params_; diff --git a/lbr_fri_ros2/test/test_position_command.cpp b/lbr_fri_ros2/test/test_position_command.cpp index 22cd7348..7da064aa 100644 --- a/lbr_fri_ros2/test/test_position_command.cpp +++ b/lbr_fri_ros2/test/test_position_command.cpp @@ -8,12 +8,11 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/filters.hpp" int main() { rclcpp::init(0, nullptr); - lbr_fri_ros2::PIDParameters pid_params; + double joint_position_tau = 0.04; lbr_fri_ros2::CommandGuardParameters cmd_guard_params; lbr_fri_ros2::StateInterfaceParameters state_interface_params; @@ -22,9 +21,8 @@ int main() { // 1. read this info!!!! from robot description - pid_params.p = 1.0; - - cmd_guard_params.joint_names = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", + "lbr_A5", "lbr_A6", "lbr_A7"}; cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., 175. * M_PI / 180.}; @@ -38,9 +36,9 @@ int main() { 200., 200., 200., 200., 200., 200., 200., }; - auto client = std::make_shared(KUKA::FRI::EClientCommandMode::POSITION, - pid_params, cmd_guard_params, "default", - state_interface_params, true); + auto client = std::make_shared( + KUKA::FRI::EClientCommandMode::POSITION, joint_position_tau, cmd_guard_params, "default", + state_interface_params, true); lbr_fri_ros2::App app(client); app.open_udp_socket(); diff --git a/lbr_fri_ros2/test/test_torque_command.cpp b/lbr_fri_ros2/test/test_torque_command.cpp index 2af7f93a..45472578 100644 --- a/lbr_fri_ros2/test/test_torque_command.cpp +++ b/lbr_fri_ros2/test/test_torque_command.cpp @@ -8,12 +8,11 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/filters.hpp" int main() { rclcpp::init(0, nullptr); - lbr_fri_ros2::PIDParameters pid_params; + double joint_position_tau = 0.04; lbr_fri_ros2::CommandGuardParameters cmd_guard_params; lbr_fri_ros2::StateInterfaceParameters state_interface_params; @@ -22,9 +21,8 @@ int main() { // 1. read this info!!!! from robot description - pid_params.p = 10.0; - - cmd_guard_params.joint_names = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", + "lbr_A5", "lbr_A6", "lbr_A7"}; cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., 175. * M_PI / 180.}; @@ -38,9 +36,9 @@ int main() { 200., 200., 200., 200., 200., 200., 200., }; - auto client = std::make_shared(KUKA::FRI::EClientCommandMode::TORQUE, - pid_params, cmd_guard_params, "default", - state_interface_params, true); + auto client = std::make_shared( + KUKA::FRI::EClientCommandMode::TORQUE, joint_position_tau, cmd_guard_params, "default", + state_interface_params, true); lbr_fri_ros2::App app(client); app.open_udp_socket(); diff --git a/lbr_fri_ros2/test/test_wrench_command.cpp b/lbr_fri_ros2/test/test_wrench_command.cpp index de88e0e6..e353476b 100644 --- a/lbr_fri_ros2/test/test_wrench_command.cpp +++ b/lbr_fri_ros2/test/test_wrench_command.cpp @@ -8,12 +8,11 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/filters.hpp" int main() { rclcpp::init(0, nullptr); - lbr_fri_ros2::PIDParameters pid_params; + double joint_position_tau = 0.04; lbr_fri_ros2::CommandGuardParameters cmd_guard_params; lbr_fri_ros2::StateInterfaceParameters state_interface_params; @@ -22,9 +21,8 @@ int main() { // 1. read this info!!!! from robot description - pid_params.p = 10.0; - - cmd_guard_params.joint_names = {"A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + cmd_guard_params.joint_names = {"lbr_A1", "lbr_A2", "lbr_A3", "lbr_A4", + "lbr_A5", "lbr_A6", "lbr_A7"}; cmd_guard_params.max_positions = {170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., 170. * M_PI / 180., 120. * M_PI / 180., 175. * M_PI / 180.}; @@ -38,9 +36,9 @@ int main() { 200., 200., 200., 200., 200., 200., 200., }; - auto client = std::make_shared(KUKA::FRI::EClientCommandMode::WRENCH, - pid_params, cmd_guard_params, "default", - state_interface_params, true); + auto client = std::make_shared( + KUKA::FRI::EClientCommandMode::WRENCH, joint_position_tau, cmd_guard_params, "default", + state_interface_params, true); lbr_fri_ros2::App app(client); app.open_udp_socket(); diff --git a/lbr_fri_ros2_stack/package.xml b/lbr_fri_ros2_stack/package.xml index bb361f9b..3b5099c2 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.2 + 2.2.0 ROS 2 stack for KUKA LBRs. mhubii Apache-2.0 diff --git a/lbr_moveit_config/doc/lbr_moveit_config.rst b/lbr_moveit_config/doc/lbr_moveit_config.rst index 3484e3a1..a4960eae 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 `_:octicon:`link-external` use the robot descriotion from ``lbr_description`` - #. In `moveit_controllers.yaml `_:octicon:`link-external` change the ``arm_controller`` to ``joint_trajectory_controller``, as in `lbr_controllers.yaml `_:octicon:`link-external` + #. In `moveit_controllers.yaml `_:octicon:`link-external` change the ``arm_controller`` to ``joint_trajectory_controller``, as in `lbr_controllers.yaml `_:octicon:`link-external` Update MoveIt Configuration --------------------------- diff --git a/lbr_moveit_config/iiwa14_moveit_config/.setup_assistant b/lbr_moveit_config/iiwa14_moveit_config/.setup_assistant index 55161cd3..ab4fdddb 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/.setup_assistant +++ b/lbr_moveit_config/iiwa14_moveit_config/.setup_assistant @@ -7,7 +7,7 @@ moveit_setup_assistant_config: package_settings: author_name: mhubii author_email: m.huber_1994@hotmail.de - generated_timestamp: 1690640386 + generated_timestamp: 1729340960 control_xacro: command: - position diff --git a/lbr_moveit_config/iiwa14_moveit_config/config/iiwa14.srdf b/lbr_moveit_config/iiwa14_moveit_config/config/iiwa14.srdf index f1b96afe..934159c3 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/config/iiwa14.srdf +++ b/lbr_moveit_config/iiwa14_moveit_config/config/iiwa14.srdf @@ -10,51 +10,51 @@ - + - - - - - - - + + + + + + + - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lbr_moveit_config/iiwa14_moveit_config/config/joint_limits.yaml b/lbr_moveit_config/iiwa14_moveit_config/config/joint_limits.yaml index ae4149e9..0ab19715 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/config/joint_limits.yaml +++ b/lbr_moveit_config/iiwa14_moveit_config/config/joint_limits.yaml @@ -8,37 +8,37 @@ 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: - A1: + lbr_A1: has_velocity_limits: true max_velocity: 1.4835298641951802 has_acceleration_limits: true max_acceleration: 10.0 - A2: + lbr_A2: has_velocity_limits: true max_velocity: 1.4835298641951802 has_acceleration_limits: true max_acceleration: 10.0 - A3: + lbr_A3: has_velocity_limits: true max_velocity: 1.7453292519943295 has_acceleration_limits: true max_acceleration: 10.0 - A4: + lbr_A4: has_velocity_limits: true max_velocity: 1.3089969389957472 has_acceleration_limits: true max_acceleration: 10.0 - A5: + lbr_A5: has_velocity_limits: true max_velocity: 2.2689280275926285 has_acceleration_limits: true max_acceleration: 10.0 - A6: + lbr_A6: has_velocity_limits: true max_velocity: 2.3561944901923448 has_acceleration_limits: true max_acceleration: 10.0 - A7: + lbr_A7: has_velocity_limits: true max_velocity: 2.3561944901923448 has_acceleration_limits: true diff --git a/lbr_moveit_config/iiwa14_moveit_config/config/moveit.rviz b/lbr_moveit_config/iiwa14_moveit_config/config/moveit.rviz index c2cac1a0..948e32b5 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/config/moveit.rviz +++ b/lbr_moveit_config/iiwa14_moveit_config/config/moveit.rviz @@ -66,51 +66,51 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_floating_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - link_1: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false - world: + Value: true + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false @@ -160,51 +160,51 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_floating_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - link_1: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false - world: + Value: true + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false 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 57d65c17..ce72820a 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/iiwa14_moveit_config/config/moveit_controllers.yaml @@ -8,13 +8,13 @@ moveit_simple_controller_manager: joint_trajectory_controller: type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true joints: - - A1 - - A2 - - A3 - - A4 - - A5 - - A6 - - A7 \ No newline at end of file + - lbr_A1 + - lbr_A2 + - lbr_A3 + - lbr_A4 + - lbr_A5 + - lbr_A6 + - lbr_A7 + action_ns: follow_joint_trajectory + default: true \ No newline at end of file diff --git a/lbr_moveit_config/iiwa14_moveit_config/package.xml b/lbr_moveit_config/iiwa14_moveit_config/package.xml index 5f3d20c0..30458809 100644 --- a/lbr_moveit_config/iiwa14_moveit_config/package.xml +++ b/lbr_moveit_config/iiwa14_moveit_config/package.xml @@ -8,11 +8,11 @@ mhubii - BSD + BSD-3-Clause http://moveit.ros.org/ - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 mhubii diff --git a/lbr_moveit_config/iiwa7_moveit_config/.setup_assistant b/lbr_moveit_config/iiwa7_moveit_config/.setup_assistant index 1b331588..70b59fa7 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/.setup_assistant +++ b/lbr_moveit_config/iiwa7_moveit_config/.setup_assistant @@ -7,7 +7,7 @@ moveit_setup_assistant_config: package_settings: author_name: mhubii author_email: m.huber_1994@hotmail.de - generated_timestamp: 1690638243 + generated_timestamp: 1729340852 control_xacro: command: - position diff --git a/lbr_moveit_config/iiwa7_moveit_config/config/iiwa7.srdf b/lbr_moveit_config/iiwa7_moveit_config/config/iiwa7.srdf index 37a9aa08..5ef397ba 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/config/iiwa7.srdf +++ b/lbr_moveit_config/iiwa7_moveit_config/config/iiwa7.srdf @@ -10,51 +10,51 @@ - + - - - - - - - + + + + + + + - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lbr_moveit_config/iiwa7_moveit_config/config/joint_limits.yaml b/lbr_moveit_config/iiwa7_moveit_config/config/joint_limits.yaml index e97eee4e..2a2bc84e 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/config/joint_limits.yaml +++ b/lbr_moveit_config/iiwa7_moveit_config/config/joint_limits.yaml @@ -8,37 +8,37 @@ 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: - A1: + lbr_A1: has_velocity_limits: true max_velocity: 1.7104226669544429 has_acceleration_limits: true max_acceleration: 10.0 - A2: + lbr_A2: has_velocity_limits: true max_velocity: 1.7104226669544429 has_acceleration_limits: true max_acceleration: 10.0 - A3: + lbr_A3: has_velocity_limits: true max_velocity: 1.7453292519943295 has_acceleration_limits: true max_acceleration: 10.0 - A4: + lbr_A4: has_velocity_limits: true max_velocity: 2.2689280275926285 has_acceleration_limits: true max_acceleration: 10.0 - A5: + lbr_A5: has_velocity_limits: true max_velocity: 2.4434609527920612 has_acceleration_limits: true max_acceleration: 10.0 - A6: + lbr_A6: has_velocity_limits: true max_velocity: 3.1415926535897931 has_acceleration_limits: true max_acceleration: 10.0 - A7: + lbr_A7: has_velocity_limits: true max_velocity: 3.1415926535897931 has_acceleration_limits: true diff --git a/lbr_moveit_config/iiwa7_moveit_config/config/moveit.rviz b/lbr_moveit_config/iiwa7_moveit_config/config/moveit.rviz index c2cac1a0..948e32b5 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/config/moveit.rviz +++ b/lbr_moveit_config/iiwa7_moveit_config/config/moveit.rviz @@ -66,51 +66,51 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_floating_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - link_1: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false - world: + Value: true + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false @@ -160,51 +160,51 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_floating_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - link_1: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false - world: + Value: true + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false 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 57d65c17..ce72820a 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/iiwa7_moveit_config/config/moveit_controllers.yaml @@ -8,13 +8,13 @@ moveit_simple_controller_manager: joint_trajectory_controller: type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true joints: - - A1 - - A2 - - A3 - - A4 - - A5 - - A6 - - A7 \ No newline at end of file + - lbr_A1 + - lbr_A2 + - lbr_A3 + - lbr_A4 + - lbr_A5 + - lbr_A6 + - lbr_A7 + action_ns: follow_joint_trajectory + default: true \ No newline at end of file diff --git a/lbr_moveit_config/iiwa7_moveit_config/package.xml b/lbr_moveit_config/iiwa7_moveit_config/package.xml index e42c1af2..c009235d 100644 --- a/lbr_moveit_config/iiwa7_moveit_config/package.xml +++ b/lbr_moveit_config/iiwa7_moveit_config/package.xml @@ -8,11 +8,11 @@ mhubii - BSD + BSD-3-Clause http://moveit.ros.org/ - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 mhubii diff --git a/lbr_moveit_config/med14_moveit_config/.setup_assistant b/lbr_moveit_config/med14_moveit_config/.setup_assistant index a9a44cf4..1d225ccc 100644 --- a/lbr_moveit_config/med14_moveit_config/.setup_assistant +++ b/lbr_moveit_config/med14_moveit_config/.setup_assistant @@ -7,7 +7,7 @@ moveit_setup_assistant_config: package_settings: author_name: mhubii author_email: m.huber_1994@hotmail.de - generated_timestamp: 1690640936 + generated_timestamp: 1729341043 control_xacro: command: - position diff --git a/lbr_moveit_config/med14_moveit_config/config/joint_limits.yaml b/lbr_moveit_config/med14_moveit_config/config/joint_limits.yaml index ae4149e9..0ab19715 100644 --- a/lbr_moveit_config/med14_moveit_config/config/joint_limits.yaml +++ b/lbr_moveit_config/med14_moveit_config/config/joint_limits.yaml @@ -8,37 +8,37 @@ 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: - A1: + lbr_A1: has_velocity_limits: true max_velocity: 1.4835298641951802 has_acceleration_limits: true max_acceleration: 10.0 - A2: + lbr_A2: has_velocity_limits: true max_velocity: 1.4835298641951802 has_acceleration_limits: true max_acceleration: 10.0 - A3: + lbr_A3: has_velocity_limits: true max_velocity: 1.7453292519943295 has_acceleration_limits: true max_acceleration: 10.0 - A4: + lbr_A4: has_velocity_limits: true max_velocity: 1.3089969389957472 has_acceleration_limits: true max_acceleration: 10.0 - A5: + lbr_A5: has_velocity_limits: true max_velocity: 2.2689280275926285 has_acceleration_limits: true max_acceleration: 10.0 - A6: + lbr_A6: has_velocity_limits: true max_velocity: 2.3561944901923448 has_acceleration_limits: true max_acceleration: 10.0 - A7: + lbr_A7: has_velocity_limits: true max_velocity: 2.3561944901923448 has_acceleration_limits: true diff --git a/lbr_moveit_config/med14_moveit_config/config/med14.srdf b/lbr_moveit_config/med14_moveit_config/config/med14.srdf index f5ef369c..1925c3e5 100644 --- a/lbr_moveit_config/med14_moveit_config/config/med14.srdf +++ b/lbr_moveit_config/med14_moveit_config/config/med14.srdf @@ -10,51 +10,51 @@ - + - - - - - - - + + + + + + + - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lbr_moveit_config/med14_moveit_config/config/moveit.rviz b/lbr_moveit_config/med14_moveit_config/config/moveit.rviz index c2cac1a0..948e32b5 100644 --- a/lbr_moveit_config/med14_moveit_config/config/moveit.rviz +++ b/lbr_moveit_config/med14_moveit_config/config/moveit.rviz @@ -66,51 +66,51 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_floating_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - link_1: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false - world: + Value: true + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false @@ -160,51 +160,51 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_floating_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - link_1: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false - world: + Value: true + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false 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 57d65c17..ce72820a 100644 --- a/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/med14_moveit_config/config/moveit_controllers.yaml @@ -8,13 +8,13 @@ moveit_simple_controller_manager: joint_trajectory_controller: type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true joints: - - A1 - - A2 - - A3 - - A4 - - A5 - - A6 - - A7 \ No newline at end of file + - lbr_A1 + - lbr_A2 + - lbr_A3 + - lbr_A4 + - lbr_A5 + - lbr_A6 + - lbr_A7 + action_ns: follow_joint_trajectory + default: true \ No newline at end of file diff --git a/lbr_moveit_config/med14_moveit_config/package.xml b/lbr_moveit_config/med14_moveit_config/package.xml index a6a6a8ce..ce1e9a0e 100644 --- a/lbr_moveit_config/med14_moveit_config/package.xml +++ b/lbr_moveit_config/med14_moveit_config/package.xml @@ -8,11 +8,11 @@ mhubii - BSD + BSD-3-Clause http://moveit.ros.org/ - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 mhubii diff --git a/lbr_moveit_config/med7_moveit_config/.setup_assistant b/lbr_moveit_config/med7_moveit_config/.setup_assistant index d788c22c..48ab6236 100644 --- a/lbr_moveit_config/med7_moveit_config/.setup_assistant +++ b/lbr_moveit_config/med7_moveit_config/.setup_assistant @@ -7,7 +7,7 @@ moveit_setup_assistant_config: package_settings: author_name: mhubii author_email: m.huber_1994@hotmail.de - generated_timestamp: 1690640668 + generated_timestamp: 1729340711 control_xacro: command: - position diff --git a/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml b/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml index e97eee4e..2a2bc84e 100644 --- a/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml +++ b/lbr_moveit_config/med7_moveit_config/config/joint_limits.yaml @@ -8,37 +8,37 @@ 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: - A1: + lbr_A1: has_velocity_limits: true max_velocity: 1.7104226669544429 has_acceleration_limits: true max_acceleration: 10.0 - A2: + lbr_A2: has_velocity_limits: true max_velocity: 1.7104226669544429 has_acceleration_limits: true max_acceleration: 10.0 - A3: + lbr_A3: has_velocity_limits: true max_velocity: 1.7453292519943295 has_acceleration_limits: true max_acceleration: 10.0 - A4: + lbr_A4: has_velocity_limits: true max_velocity: 2.2689280275926285 has_acceleration_limits: true max_acceleration: 10.0 - A5: + lbr_A5: has_velocity_limits: true max_velocity: 2.4434609527920612 has_acceleration_limits: true max_acceleration: 10.0 - A6: + lbr_A6: has_velocity_limits: true max_velocity: 3.1415926535897931 has_acceleration_limits: true max_acceleration: 10.0 - A7: + lbr_A7: has_velocity_limits: true max_velocity: 3.1415926535897931 has_acceleration_limits: true diff --git a/lbr_moveit_config/med7_moveit_config/config/med7.srdf b/lbr_moveit_config/med7_moveit_config/config/med7.srdf index 1b4932a5..63e98c43 100644 --- a/lbr_moveit_config/med7_moveit_config/config/med7.srdf +++ b/lbr_moveit_config/med7_moveit_config/config/med7.srdf @@ -10,51 +10,51 @@ - + - - - - - - - + + + + + + + - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lbr_moveit_config/med7_moveit_config/config/moveit.rviz b/lbr_moveit_config/med7_moveit_config/config/moveit.rviz index c2cac1a0..948e32b5 100644 --- a/lbr_moveit_config/med7_moveit_config/config/moveit.rviz +++ b/lbr_moveit_config/med7_moveit_config/config/moveit.rviz @@ -66,51 +66,51 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_floating_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - link_1: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false - world: + Value: true + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false @@ -160,51 +160,51 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - link_0: + lbr_floating_link: Alpha: 1 Show Axes: false Show Trail: false - Value: true - link_1: + lbr_link_0: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_2: + lbr_link_1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_3: + lbr_link_2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_4: + lbr_link_3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_5: + lbr_link_4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_6: + lbr_link_5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_7: + lbr_link_6: Alpha: 1 Show Axes: false Show Trail: false Value: true - link_ee: + lbr_link_7: Alpha: 1 Show Axes: false Show Trail: false - world: + Value: true + lbr_link_ee: Alpha: 1 Show Axes: false Show Trail: false 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 57d65c17..ce72820a 100644 --- a/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml +++ b/lbr_moveit_config/med7_moveit_config/config/moveit_controllers.yaml @@ -8,13 +8,13 @@ moveit_simple_controller_manager: joint_trajectory_controller: type: FollowJointTrajectory - action_ns: follow_joint_trajectory - default: true joints: - - A1 - - A2 - - A3 - - A4 - - A5 - - A6 - - A7 \ No newline at end of file + - lbr_A1 + - lbr_A2 + - lbr_A3 + - lbr_A4 + - lbr_A5 + - lbr_A6 + - lbr_A7 + action_ns: follow_joint_trajectory + default: true \ No newline at end of file diff --git a/lbr_moveit_config/med7_moveit_config/package.xml b/lbr_moveit_config/med7_moveit_config/package.xml index dd007600..a391df42 100644 --- a/lbr_moveit_config/med7_moveit_config/package.xml +++ b/lbr_moveit_config/med7_moveit_config/package.xml @@ -8,11 +8,11 @@ mhubii - BSD + BSD-3-Clause http://moveit.ros.org/ - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 mhubii diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index 6dc55918..837382d2 100644 --- a/lbr_ros2_control/CMakeLists.txt +++ b/lbr_ros2_control/CMakeLists.txt @@ -16,11 +16,13 @@ endif() find_package(ament_cmake REQUIRED) find_package(controller_interface REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3) find_package(FRIClient REQUIRED) find_package(hardware_interface REQUIRED) +find_package(kinematics_interface REQUIRED) find_package(lbr_fri_idl REQUIRED) find_package(lbr_fri_ros2 REQUIRED) -find_package(kinematics_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(realtime_tools REQUIRED) @@ -29,10 +31,12 @@ find_package(realtime_tools REQUIRED) add_library( ${PROJECT_NAME} SHARED + src/controllers/admittance_controller.cpp src/controllers/lbr_joint_position_command_controller.cpp src/controllers/lbr_torque_command_controller.cpp src/controllers/lbr_wrench_command_controller.cpp src/controllers/lbr_state_broadcaster.cpp + src/controllers/twist_controller.cpp src/system_interface.cpp ) @@ -47,16 +51,21 @@ target_include_directories( ) # Link against dependencies -ament_target_dependencies( - ${PROJECT_NAME} +set(AMENT_DEPENDENCIES controller_interface + Eigen3 hardware_interface + kinematics_interface lbr_fri_idl lbr_fri_ros2 pluginlib rclcpp realtime_tools ) +ament_target_dependencies( + ${PROJECT_NAME} + ${AMENT_DEPENDENCIES} +) target_link_libraries(${PROJECT_NAME} FRIClient::FRIClient @@ -71,16 +80,11 @@ ament_export_targets( ) ament_export_dependencies( - controller_interface FRIClient - hardware_interface - lbr_fri_idl - lbr_fri_ros2 - pluginlib - rclcpp - realtime_tools + ${AMENT_DEPENDENCIES} + eigen3_cmake_module ) - + install( DIRECTORY include/ DESTINATION include @@ -94,9 +98,4 @@ install( INCLUDES DESTINATION include ) -install( - DIRECTORY config - DESTINATION share/${PROJECT_NAME} -) - ament_package() diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml deleted file mode 100644 index c7939d25..00000000 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ /dev/null @@ -1,72 +0,0 @@ -# Use of /** so that the configurations hold for controller -# managers regardless of their namespace. Usefull in multi-robot setups. -/**/controller_manager: - ros__parameters: - update_rate: 200 - - # ROS 2 control broadcasters - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - - force_torque_broadcaster: - type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster - - # LBR ROS 2 control broadcasters - lbr_state_broadcaster: - type: lbr_ros2_control/LBRStateBroadcaster - - # ROS 2 control controllers - joint_trajectory_controller: - type: joint_trajectory_controller/JointTrajectoryController - - forward_position_controller: - type: position_controllers/JointGroupPositionController - - # LBR ROS 2 control controllers - lbr_joint_position_command_controller: - type: lbr_ros2_control/LBRJointPositionCommandController - - lbr_torque_command_controller: - type: lbr_ros2_control/LBRTorqueCommandController - - lbr_wrench_command_controller: - type: lbr_ros2_control/LBRWrenchCommandController - - # Admittance controller - admittance_controller: - type: lbr_ros2_control/AdmittanceController - -/**/force_torque_broadcaster: - ros__parameters: - frame_id: lbr/link_ee # namespace: https://github.com/ros2/rviz/issues/1103 - sensor_name: estimated_ft_sensor - -/**/joint_trajectory_controller: - ros__parameters: - joints: - - A1 - - A2 - - A3 - - A4 - - A5 - - A6 - - A7 - command_interfaces: - - position - state_interfaces: - - position - - velocity - state_publish_rate: 50.0 - action_monitor_rate: 20.0 - -/**/forward_position_controller: - ros__parameters: - joints: - - A1 - - A2 - - A3 - - A4 - - A5 - - A6 - - A7 - interface_name: position diff --git a/lbr_ros2_control/doc/img/lbr_ros2_control_detailed_v2.0.0.svg b/lbr_ros2_control/doc/img/lbr_ros2_control_detailed_v2.0.0.svg index 1ea801a2..939e9932 100644 --- a/lbr_ros2_control/doc/img/lbr_ros2_control_detailed_v2.0.0.svg +++ b/lbr_ros2_control/doc/img/lbr_ros2_control_detailed_v2.0.0.svg @@ -1,4 +1,4 @@ -
lbr_fri_ros2
lbr_fri_ros2::App
Runs the lbr_fri_ros2::AsyncClient at the robot's sample time. This sample time can be different from the controller_manager's update_rate!
lbr_fri_ros2::AsyncClient
The client with different interfaces, depending on the robot's command mode.
1
lbr_ros2_control
lbr_ros2_control::SystemInterface
The lbr_ros2_control::SystemInterface has an lbr_fri_ros2::App for communicating with the robot. It further implements the hardware_interface::SystemInterface interfaces for ROS 2 control integration.
1
1
1
1
controller_manager::ControllerManager
The controller_manager::ControllerManager loads the lbr_ros2_control::SystemInterface at runtime. It has a shared instance of the controller_manager node.
controller_manager

This node is run at update_rate.
hardware_interface::SystemInterface
Base class for ROS 2 control system plugins.
parent
child
lbr_system_paramters yaml
Robot configurations such as control mode, port_id, and others
Loads
1
Requests
1
lbr_system_interface.xacro
This xacro file tells the controller_manager::ControllerManager to load the lbr_ros2_control::SystemInterface.

It loads system_parameters.yaml and forwards the configurations to lbr_ros2_control::SystemInterface.
robot_description
 \ No newline at end of file +
lbr_fri_ros2
lbr_fri_ros2::App
Runs the lbr_fri_ros2::AsyncClient at the robot's sample time. This sample time can be different from the controller_manager's update_rate!
lbr_fri_ros2::AsyncClient
The client with different interfaces, depending on the robot's command mode.
1
lbr_ros2_control
lbr_ros2_control::SystemInterface
The lbr_ros2_control::SystemInterface has an lbr_fri_ros2::App for communicating with the robot. It further implements the hardware_interface::SystemInterface interfaces for ROS 2 control integration.
1
1
1
1
controller_manager::ControllerManager
The controller_manager::ControllerManager loads the lbr_ros2_control::SystemInterface at runtime. It has a shared instance of the controller_manager node.
controller_manager

This node is run at update_rate.
hardware_interface::SystemInterface
Base class for ROS 2 control system plugins.
parent
child
lbr_system_paramters yaml
Robot configurations such as control mode, port_id, and others
Loads
1
Requests
1
lbr_system_interface.xacro
This xacro file tells the controller_manager::ControllerManager to load the lbr_ros2_control::SystemInterface.

It loads lbr_system_config.yaml and forwards the configurations to lbr_ros2_control::SystemInterface.
robot_description
 \ No newline at end of file diff --git a/lbr_ros2_control/doc/img/lbr_ros2_control_v2.0.0.svg b/lbr_ros2_control/doc/img/lbr_ros2_control_v2.0.0.svg index 86cfb20d..4406d270 100644 --- a/lbr_ros2_control/doc/img/lbr_ros2_control_v2.0.0.svg +++ b/lbr_ros2_control/doc/img/lbr_ros2_control_v2.0.0.svg @@ -1,4 +1,4 @@ -
lbr_system_parameters.yaml
controller_manager::ControllerManager
Robot
UDP Connection
lbr_ros2_control::SystemInterface
lbr_fri_ros2::App
Runs asynchronously
at robot's sample time
  • Owns:
    • Controllers
    • Hardware plugins, i.e.
      lbr_ros2_control::SystemInterface
  • Runs at update_rate
Controller A
Controller B
 \ No newline at end of file +
lbr_system_config.yaml
controller_manager::ControllerManager
Robot
UDP Connection
lbr_ros2_control::SystemInterface
lbr_fri_ros2::App
Runs asynchronously
at robot's sample time
  • Owns:
    • Controllers
    • Hardware plugins, i.e.
      lbr_ros2_control::SystemInterface
  • Runs at update_rate
Controller A
Controller B
 \ No newline at end of file diff --git a/lbr_ros2_control/doc/lbr_ros2_control.rst b/lbr_ros2_control/doc/lbr_ros2_control.rst index 36f2f7ba..d3fe9d84 100644 --- a/lbr_ros2_control/doc/lbr_ros2_control.rst +++ b/lbr_ros2_control/doc/lbr_ros2_control.rst @@ -30,7 +30,7 @@ Hardware components and controllers are loaded as plugins (components) by the `` The ``controller_manager::ControllerManager`` has a node, the `controller_manager `_:octicon:`link-external`. - Hardware plugins are read from the ``robot_descritption`` parameter of the ``robot_state_publisher`` node and loaded at runtime. -- Parameters, such as ``update_rate``, the configured controllers, are simply set as node parameters, see e.g. `lbr_controllers.yaml `_:octicon:`link-external`. +- Parameters, such as ``update_rate``, the configured controllers, are simply set as node parameters, see e.g. `lbr_controllers.yaml `_:octicon:`link-external`. Hardware Plugin --------------- @@ -42,8 +42,8 @@ The ``lbr_ros2_control::SystemInterface`` plugin implements a ``hardware_interfa .. thumbnail:: img/lbr_ros2_control_detailed_v2.0.0.svg :alt: lbr_ros2_control_detailed -- The ``controller_manager::ControllerManager`` loads the correct plugin from the ```` tag in the ``robot_description``: `lbr_system_interface.xacro `_:octicon:`link-external` -- FRI relevant parameters are forwarded to the ``lbr_ros2_control::SystemInterface`` plugin from `lbr_system_paramters.yaml `_:octicon:`link-external` +- The ``controller_manager::ControllerManager`` loads the correct plugin from the ```` tag in the ``robot_description``: `lbr_system_interface.xacro `_:octicon:`link-external` +- FRI relevant parameters are forwarded to the ``lbr_ros2_control::SystemInterface`` plugin from `lbr_system_config.yaml `_:octicon:`link-external` The ``lbr_ros2_control::SystemInterface`` is spun with the ``controller_manager`` node at a rate set by the ``update_rate`` parameter. The communication to the robot is run **asynchronously** at a rate set by the robot's sample time. diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp index 6d28e3a1..7488d6b2 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp @@ -9,21 +9,19 @@ #include #include "controller_interface/controller_interface.hpp" -#include "hardware_interface/loaned_command_interface.hpp" +#include "eigen3/Eigen/Core" +#include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" +#include "semantic_components/force_torque_sensor.hpp" #include "friLBRState.h" +#include "lbr_fri_ros2/control.hpp" +#include "lbr_fri_ros2/types.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { -class Admittance { - -} - class AdmittanceController : public controller_interface::ControllerInterface { - static constexpr uint8_t CARTESIAN_DOF = 6; - public: AdmittanceController(); @@ -46,18 +44,37 @@ class AdmittanceController : public controller_interface::ControllerInterface { on_deactivate(const rclcpp_lifecycle::State &previous_state) override; protected: - bool reference_command_interfaces_(); bool reference_state_interfaces_(); - void clear_command_interfaces_(); void clear_state_interfaces_(); - - std::array joint_names_ = { - "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; - - std::vector> - joint_position_command_interfaces_; + void configure_joint_names_(); + void configure_admittance_impl_(); + void configure_inv_jac_ctrl_impl_(); + void zero_all_values_(); + void log_info_() const; + + // robot description + std::string robot_description_; + + // admittance + bool initialized_ = false; + std::unique_ptr admittance_impl_ptr_; + Eigen::Matrix x_init_, x_prev_; + Eigen::Matrix f_ext_, x_, dx_, ddx_; + + // joint veloctiy computation + std::unique_ptr inv_jac_ctrl_impl_ptr_; + lbr_fri_ros2::jnt_array_t q_, dq_; + Eigen::Matrix twist_command_; + + // interfaces + lbr_fri_ros2::jnt_name_array_t joint_names_; std::vector> - estimated_ft_sensor_state_interface_; + joint_position_state_interfaces_; + std::unique_ptr> + sample_time_state_interface_ptr_; + std::unique_ptr> + session_state_interface_ptr_; + std::unique_ptr estimated_ft_sensor_ptr_; }; } // namespace lbr_ros2_control #endif // LBR_ROS2_CONTROL__ADMITTANCE_CONTROLLER_HPP_ diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp index 46eebecd..12bd9d00 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp @@ -15,6 +15,7 @@ #include "friLBRState.h" #include "lbr_fri_idl/msg/lbr_joint_position_command.hpp" +#include "lbr_fri_ros2/types.hpp" namespace lbr_ros2_control { class LBRJointPositionCommandController : public controller_interface::ControllerInterface { @@ -40,8 +41,9 @@ class LBRJointPositionCommandController : public controller_interface::Controlle on_deactivate(const rclcpp_lifecycle::State &previous_state) override; protected: - std::array joint_names_ = { - "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + void configure_joint_names_(); + + lbr_fri_ros2::jnt_name_array_t joint_names_; realtime_tools::RealtimeBuffer rt_lbr_joint_position_command_ptr_; diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp index 877f71e6..c89d564b 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp @@ -19,6 +19,7 @@ #include "friLBRState.h" #include "lbr_fri_idl/msg/lbr_state.hpp" +#include "lbr_fri_ros2/types.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { @@ -47,9 +48,9 @@ class LBRStateBroadcaster : public controller_interface::ControllerInterface { protected: void init_state_interface_map_(); void init_state_msg_(); + void configure_joint_names_(); - std::array joint_names_ = { - "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + lbr_fri_ros2::jnt_name_array_t joint_names_; std::unordered_map> state_interface_map_; rclcpp::Publisher::SharedPtr state_publisher_ptr_; diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp index 7a4c5f7b..8094e065 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp @@ -17,6 +17,7 @@ #include "friLBRState.h" #include "lbr_fri_idl/msg/lbr_torque_command.hpp" +#include "lbr_fri_ros2/types.hpp" namespace lbr_ros2_control { class LBRTorqueCommandController : public controller_interface::ControllerInterface { @@ -44,9 +45,9 @@ class LBRTorqueCommandController : public controller_interface::ControllerInterf protected: bool reference_command_interfaces_(); void clear_command_interfaces_(); + void configure_joint_names_(); - std::array joint_names_ = { - "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + lbr_fri_ros2::jnt_name_array_t joint_names_; std::vector> joint_position_command_interfaces_, torque_command_interfaces_; diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp index 52b76f35..8f43904f 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp @@ -17,6 +17,7 @@ #include "friLBRState.h" #include "lbr_fri_idl/msg/lbr_wrench_command.hpp" +#include "lbr_fri_ros2/types.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { @@ -47,9 +48,9 @@ class LBRWrenchCommandController : public controller_interface::ControllerInterf protected: bool reference_command_interfaces_(); void clear_command_interfaces_(); + void configure_joint_names_(); - std::array joint_names_ = { - "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + lbr_fri_ros2::jnt_name_array_t joint_names_; std::vector> joint_position_command_interfaces_, wrench_command_interfaces_; diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp new file mode 100644 index 00000000..6f202520 --- /dev/null +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp @@ -0,0 +1,82 @@ +#ifndef LBR_ROS2_CONTROL__TWIST_CONTROLLER_HPP_ +#define LBR_ROS2_CONTROL__TWIST_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" +#include "realtime_tools/realtime_buffer.h" + +#include "friLBRState.h" + +#include "lbr_fri_ros2/control.hpp" +#include "lbr_fri_ros2/kinematics.hpp" +#include "lbr_fri_ros2/types.hpp" +#include "lbr_ros2_control/system_interface_type_values.hpp" + +namespace lbr_ros2_control { +class TwistController : public controller_interface::ControllerInterface { +public: + TwistController(); + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::CallbackReturn on_init() override; + + controller_interface::return_type update(const rclcpp::Time &time, + const rclcpp::Duration &period) override; + + controller_interface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &previous_state) override; + + controller_interface::CallbackReturn + on_activate(const rclcpp_lifecycle::State &previous_state) override; + + controller_interface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + +protected: + bool reference_state_interfaces_(); + void clear_state_interfaces_(); + void reset_command_buffer_(); + void configure_joint_names_(); + void configure_inv_jac_ctrl_impl_(); + void log_info_() const; + + // robot description + std::string robot_description_; + + // some safety checks + std::atomic updates_since_last_command_ = 0; + double timeout_ = 0.2; + + // joint veloctiy computation + std::unique_ptr inv_jac_ctrl_impl_ptr_; + lbr_fri_ros2::jnt_array_t q_, dq_; + + // interfaces + lbr_fri_ros2::jnt_name_array_t joint_names_; + std::vector> + joint_position_state_interfaces_; + std::unique_ptr> + sample_time_state_interface_ptr_; + std::unique_ptr> + session_state_interface_ptr_; + + // real-time twist command topic + realtime_tools::RealtimeBuffer rt_twist_ptr_; + rclcpp::Subscription::SharedPtr twist_subscription_ptr_; +}; +} // namespace lbr_ros2_control +#endif // LBR_ROS2_CONTROL__TWIST_CONTROLLER_HPP_ diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index ceb5157c..a7448a3c 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -23,10 +23,10 @@ #include "lbr_fri_ros2/app.hpp" #include "lbr_fri_ros2/async_client.hpp" #include "lbr_fri_ros2/command_guard.hpp" -#include "lbr_fri_ros2/filters.hpp" #include "lbr_fri_ros2/formatting.hpp" #include "lbr_fri_ros2/ft_estimator.hpp" #include "lbr_fri_ros2/interfaces/state.hpp" +#include "lbr_fri_ros2/types.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { @@ -43,20 +43,18 @@ struct SystemInterfaceParameters { const char *remote_host{nullptr}; int32_t rt_prio{80}; bool open_loop{true}; - double pid_p{0.0}; - double pid_i{0.0}; - double pid_d{0.0}; - double pid_i_max{0.0}; - double pid_i_min{0.0}; - double pid_antiwindup{0.0}; + double joint_position_tau{0.04}; std::string command_guard_variant{"default"}; - double external_torque_cutoff_frequency{10.0}; - double measured_torque_cutoff_frequency{10.0}; + double external_torque_tau{0.04}; + double measured_torque_tau{0.04}; }; struct EstimatedFTSensorParameters { - std::string chain_root{"link_0"}; - std::string chain_tip{"link_ee"}; + bool enabled{true}; + std::uint16_t update_rate{100}; + int32_t rt_prio{30}; + std::string chain_root{"lbr_link_0"}; + std::string chain_tip{"lbr_link_ee"}; double damping{0.2}; double force_x_th{2.0}; double force_y_th{2.0}; @@ -160,7 +158,8 @@ class SystemInterface : public hardware_interface::SystemInterface { void compute_hw_velocity_(); // additional force-torque state interface - lbr_fri_ros2::FTEstimator::cart_array_t hw_ft_; + lbr_fri_ros2::cart_array_t hw_ft_; + std::shared_ptr ft_estimator_impl_ptr_; std::unique_ptr ft_estimator_ptr_; // exposed command interfaces diff --git a/lbr_ros2_control/package.xml b/lbr_ros2_control/package.xml index f6f5b81a..d7cfe74f 100644 --- a/lbr_ros2_control/package.xml +++ b/lbr_ros2_control/package.xml @@ -2,24 +2,31 @@ lbr_ros2_control - 2.1.2 + 2.2.0 ROS 2 hardware hardware_interface for KUKA LBR through Fast Robot Interface (FRI). mhubii Apache-2.0 ament_cmake + eigen3_cmake_module + + eigen fri_client_sdk + geometry_msgs lbr_fri_idl lbr_fri_ros2 pluginlib rclcpp realtime_tools ros2_control - + lbr_description ros2_controllers + eigen3_cmake_module + eigen + ament_cmake diff --git a/lbr_ros2_control/plugin_description_files/controllers.xml b/lbr_ros2_control/plugin_description_files/controllers.xml index 82c4ae8e..10694639 100644 --- a/lbr_ros2_control/plugin_description_files/controllers.xml +++ b/lbr_ros2_control/plugin_description_files/controllers.xml @@ -1,9 +1,10 @@ - - + - Broadcaster for LBRState messages, see lbr_fri_idl/msg/LBRState.msg. + A simple admittance controller. @@ -14,6 +15,12 @@ lbr_fri_idl/msg/LBRJointPositionCommand.msg. + + + Broadcaster for LBRState messages, see lbr_fri_idl/msg/LBRState.msg. + + - - + - A simple admittance controller. + A simple twist controller. \ No newline at end of file diff --git a/lbr_ros2_control/src/controllers/admittance_controller.cpp b/lbr_ros2_control/src/controllers/admittance_controller.cpp index 147ad742..2b6c0b45 100644 --- a/lbr_ros2_control/src/controllers/admittance_controller.cpp +++ b/lbr_ros2_control/src/controllers/admittance_controller.cpp @@ -4,74 +4,343 @@ namespace lbr_ros2_control { AdmittanceController::AdmittanceController() {} controller_interface::InterfaceConfiguration -AdmittanceController::command_interface_configuration() { - // reference joint position command interface +AdmittanceController::command_interface_configuration() const { + controller_interface::InterfaceConfiguration interface_configuration; + interface_configuration.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto &joint_name : joint_names_) { + interface_configuration.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION); + } + return interface_configuration; } -controller_interface::InterfaceConfiguration AdmittanceController::state_interface_configuration() { - // retrieve estimated ft state interface +controller_interface::InterfaceConfiguration +AdmittanceController::state_interface_configuration() const { + controller_interface::InterfaceConfiguration interface_configuration; + interface_configuration.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + // joint position interface + for (const auto &joint_name : joint_names_) { + interface_configuration.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION); + } + + // estimated force-torque sensor interface + for (const auto &interface_name : estimated_ft_sensor_ptr_->get_state_interface_names()) { + interface_configuration.names.push_back(interface_name); + } + + // additional state interfaces + interface_configuration.names.push_back(std::string(HW_IF_AUXILIARY_PREFIX) + "/" + + HW_IF_SAMPLE_TIME); + interface_configuration.names.push_back(std::string(HW_IF_AUXILIARY_PREFIX) + "/" + + HW_IF_SESSION_STATE); + return interface_configuration; } -controller_interface::CallbackReturn AdmittanceController::on_init() {} +controller_interface::CallbackReturn AdmittanceController::on_init() { + try { + if (!this->get_node()->has_parameter("robot_description")) { + this->get_node()->declare_parameter("robot_description", ""); + } + if (!this->get_node()->has_parameter("robot_name")) { + this->get_node()->declare_parameter("robot_name", "lbr"); + } + if (!this->get_node()->has_parameter("admittance.mass")) { + this->get_node()->declare_parameter("admittance.mass", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 1.0)); + } + if (!this->get_node()->has_parameter("admittance.damping")) { + this->get_node()->declare_parameter("admittance.damping", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); + } + if (!this->get_node()->has_parameter("admittance.stiffness")) { + this->get_node()->declare_parameter("admittance.stiffness", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.chain_root")) { + this->get_node()->declare_parameter("inv_jac_ctrl.chain_root", "lbr_link_0"); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.chain_tip")) { + this->get_node()->declare_parameter("inv_jac_ctrl.chain_tip", "lbr_link_ee"); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.damping")) { + this->get_node()->declare_parameter("inv_jac_ctrl.damping", 0.2); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.max_linear_velocity")) { + this->get_node()->declare_parameter("inv_jac_ctrl.max_linear_velocity", 0.1); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.max_angular_velocity")) { + this->get_node()->declare_parameter("inv_jac_ctrl.max_angular_velocity", 0.1); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.joint_gains")) { + this->get_node()->declare_parameter("inv_jac_ctrl.joint_gains", + std::vector(lbr_fri_ros2::N_JNTS, 0.0)); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.cartesian_gains")) { + this->get_node()->declare_parameter("inv_jac_ctrl.cartesian_gains", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); + } + robot_description_ = this->get_node()->get_parameter("robot_description").as_string(); + if (robot_description_.empty()) { + throw std::runtime_error("No robot description provided"); + } + configure_joint_names_(); + configure_admittance_impl_(); + configure_inv_jac_ctrl_impl_(); + log_info_(); + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_node()->get_logger(), + "Failed to initialize admittance controller with: %s.", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} -controller_interface::return_type AdmittanceController::update(const rclcpp::Time &time, +controller_interface::return_type AdmittanceController::update(const rclcpp::Time & /*time*/, const rclcpp::Duration &period) { + // get estimated force-torque sensor values + f_ext_.head(3) = + Eigen::Map>(estimated_ft_sensor_ptr_->get_forces().data()); + f_ext_.tail(3) = + Eigen::Map>(estimated_ft_sensor_ptr_->get_torques().data()); + + // get joint positions + std::for_each(q_.begin(), q_.end(), [&, i = 0](double &q_i) mutable { + q_i = this->state_interfaces_[i].get_value(); + ++i; + }); + + // compute forward kinematics + auto chain_tip_frame = inv_jac_ctrl_impl_ptr_->get_kinematics_ptr()->compute_fk(q_); + x_.head(3) = Eigen::Map>(chain_tip_frame.p.data); + x_.tail(3) = Eigen::Map>(chain_tip_frame.M.GetRot().data); + + // compute steady state position and orientation + if (!initialized_) { + x_init_ = x_; + x_prev_ = x_; + initialized_ = true; + } + + // compute velocity & update previous position + dx_ = (x_ - x_prev_) / period.seconds(); + x_prev_ = x_; + + // convert f_ext_ back to root frame + f_ext_.head(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * f_ext_.head(3); + f_ext_.tail(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * f_ext_.tail(3); + // compute admittance - // add warning for high force-torques and refer to load data calibration + admittance_impl_ptr_->compute(f_ext_, x_ - x_init_, dx_, ddx_); + + // integrate ddx_ to command velocity + twist_command_ = ddx_ * period.seconds(); + + if (!inv_jac_ctrl_impl_ptr_) { + RCLCPP_ERROR(this->get_node()->get_logger(), "Inverse Jacobian controller not initialized."); + return controller_interface::return_type::ERROR; + } + if (static_cast(session_state_interface_ptr_->get().get_value()) != + KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { + return controller_interface::return_type::OK; + } + + // compute the joint velocity from the twist command target + inv_jac_ctrl_impl_ptr_->compute(twist_command_, q_, dq_); + + // pass joint positions to hardware + std::for_each(q_.begin(), q_.end(), [&, i = 0](const double &q_i) mutable { + this->command_interfaces_[i].set_value( + q_i + dq_[i] * sample_time_state_interface_ptr_->get().get_value()); + ++i; + }); + + return controller_interface::return_type::OK; } controller_interface::CallbackReturn -AdmittanceController::on_configure(const rclcpp_lifecycle::State &previous_state) { +AdmittanceController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + estimated_ft_sensor_ptr_ = std::make_unique( + std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_FORCE_X, + std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_FORCE_Y, + std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_FORCE_Z, + std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_TORQUE_X, + std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_TORQUE_Y, + std::string(HW_IF_ESTIMATED_FT_PREFIX) + "/" + HW_IF_TORQUE_Z); return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn -AdmittanceController::on_activate(const rclcpp_lifecycle::State &previous_state) { - if (!reference_command_interfaces_()) { - return controller_interface::CallbackReturn::ERROR; - } +AdmittanceController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { if (!reference_state_interfaces_()) { return controller_interface::CallbackReturn::ERROR; } + zero_all_values_(); return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn -AdmittanceController::on_deactivate(const rclcpp_lifecycle::State &previous_state) { - clear_command_interfaces_(); +AdmittanceController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { clear_state_interfaces_(); return controller_interface::CallbackReturn::SUCCESS; } -bool AdmittanceController::reference_command_interfaces_() { - for (auto &command_interface : command_interfaces_) { - if (command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { - joint_position_command_interfaces_.emplace_back(std::ref(command_interface)); +bool AdmittanceController::reference_state_interfaces_() { + for (auto &state_interface : state_interfaces_) { + if (state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { + joint_position_state_interfaces_.emplace_back(std::ref(state_interface)); + } + if (state_interface.get_interface_name() == HW_IF_SAMPLE_TIME) { + sample_time_state_interface_ptr_ = + std::make_unique>( + std::ref(state_interface)); + } + if (state_interface.get_interface_name() == HW_IF_SESSION_STATE) { + session_state_interface_ptr_ = + std::make_unique>( + std::ref(state_interface)); } } - if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (!estimated_ft_sensor_ptr_->assign_loaned_state_interfaces(state_interfaces_)) { + RCLCPP_ERROR(this->get_node()->get_logger(), + "Failed to assign estimated force torque state interfaces."); + return false; + } + if (joint_position_state_interfaces_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), - "Number of joint position command interfaces '%ld' does not match the number of joints " + "Number of joint position state interfaces '%ld' does not match the number of joints " "in the robot '%d'.", - joint_position_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + joint_position_state_interfaces_.size(), lbr_fri_ros2::N_JNTS); return false; } + return true; } -bool AdmittanceController::reference_state_interfaces_() { - for (auto &state_interface : state_interfaces_) { - if (state_interface.get_interface_name() == HW_IF_ESTIMATED_FT_PREFIX) { - estimated_ft_sensor_state_interface_.emplace_back(std::ref(state_interface)); - } +void AdmittanceController::clear_state_interfaces_() { + joint_position_state_interfaces_.clear(); + estimated_ft_sensor_ptr_->release_interfaces(); +} + +void AdmittanceController::configure_joint_names_() { + if (joint_names_.size() != lbr_fri_ros2::N_JNTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + joint_names_.size(), lbr_fri_ros2::N_JNTS); + throw std::runtime_error("Failed to configure joint names."); + } + std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); + for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); } } -void AdmittanceController::clear_command_interfaces_() { - joint_position_command_interfaces_.clear(); +void AdmittanceController::configure_admittance_impl_() { + if (this->get_node()->get_parameter("admittance.mass").as_double_array().size() != + lbr_fri_ros2::CARTESIAN_DOF) { + RCLCPP_ERROR(this->get_node()->get_logger(), + "Number of mass values (%ld) does not match the number of cartesian degrees of " + "freedom (%d).", + this->get_node()->get_parameter("admittance.mass").as_double_array().size(), + lbr_fri_ros2::CARTESIAN_DOF); + throw std::runtime_error("Failed to configure admittance parameters."); + } + if (this->get_node()->get_parameter("admittance.damping").as_double_array().size() != + lbr_fri_ros2::CARTESIAN_DOF) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of damping values (%ld) does not match the number of cartesian degrees of freedom " + "(%d).", + this->get_node()->get_parameter("admittance.damping").as_double_array().size(), + lbr_fri_ros2::CARTESIAN_DOF); + throw std::runtime_error("Failed to configure admittance parameters."); + } + if (this->get_node()->get_parameter("admittance.stiffness").as_double_array().size() != + lbr_fri_ros2::CARTESIAN_DOF) { + RCLCPP_ERROR(this->get_node()->get_logger(), + "Number of stiffness values (%ld) does not match the number of cartesian degrees " + "of freedom " + "(%d).", + this->get_node()->get_parameter("admittance.stiffness").as_double_array().size(), + lbr_fri_ros2::CARTESIAN_DOF); + throw std::runtime_error("Failed to configure admittance parameters."); + } + lbr_fri_ros2::cart_array_t mass_array; + for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { + mass_array[i] = this->get_node()->get_parameter("admittance.mass").as_double_array()[i]; + } + lbr_fri_ros2::cart_array_t damping_array; + for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { + damping_array[i] = this->get_node()->get_parameter("admittance.damping").as_double_array()[i]; + } + lbr_fri_ros2::cart_array_t stiffness_array; + for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { + stiffness_array[i] = + this->get_node()->get_parameter("admittance.stiffness").as_double_array()[i]; + } + admittance_impl_ptr_ = std::make_unique( + lbr_fri_ros2::AdmittanceParameters{mass_array, damping_array, stiffness_array}); } -void AdmittanceController::clear_state_interfaces_() { - estimated_ft_sensor_state_interface_.clear(); +void AdmittanceController::configure_inv_jac_ctrl_impl_() { + if (this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array().size() != + lbr_fri_ros2::N_JNTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint gains (%ld) does not match the number of joints in the robot (%d).", + this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array().size(), + lbr_fri_ros2::N_JNTS); + throw std::runtime_error("Failed to configure joint gains."); + } + if (this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array().size() != + lbr_fri_ros2::CARTESIAN_DOF) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of cartesian gains (%ld) does not match the number of cartesian degrees of freedom " + "(%d).", + this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array().size(), + lbr_fri_ros2::CARTESIAN_DOF); + throw std::runtime_error("Failed to configure cartesian gains."); + } + lbr_fri_ros2::jnt_array_t joint_gains_array; + for (unsigned int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + joint_gains_array[i] = + this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array()[i]; + } + lbr_fri_ros2::cart_array_t cartesian_gains_array; + for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { + cartesian_gains_array[i] = + this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array()[i]; + } + inv_jac_ctrl_impl_ptr_ = std::make_unique( + robot_description_, + lbr_fri_ros2::InvJacCtrlParameters{ + this->get_node()->get_parameter("inv_jac_ctrl.chain_root").as_string(), + this->get_node()->get_parameter("inv_jac_ctrl.chain_tip").as_string(), + false, // always assume twist in root frame + this->get_node()->get_parameter("inv_jac_ctrl.damping").as_double(), + this->get_node()->get_parameter("inv_jac_ctrl.max_linear_velocity").as_double(), + this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double(), + joint_gains_array, cartesian_gains_array}); +} + +void AdmittanceController::zero_all_values_() { + f_ext_.setZero(); + x_.setZero(); + dx_.setZero(); + ddx_.setZero(); + std::fill(dq_.begin(), dq_.end(), 0.0); + twist_command_.setZero(); +} + +void AdmittanceController::log_info_() const { + admittance_impl_ptr_->log_info(); + inv_jac_ctrl_impl_ptr_->log_info(); } } // namespace lbr_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::AdmittanceController, + controller_interface::ControllerInterface) \ No newline at end of file diff --git a/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp index f5217b82..d78d825b 100644 --- a/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp @@ -29,6 +29,10 @@ controller_interface::CallbackReturn LBRJointPositionCommandController::on_init( [this](const lbr_fri_idl::msg::LBRJointPositionCommand::SharedPtr msg) { rt_lbr_joint_position_command_ptr_.writeFromNonRT(msg); }); + if (!this->get_node()->has_parameter("robot_name")) { + this->get_node()->declare_parameter("robot_name", "lbr"); + } + configure_joint_names_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize LBR position command controller with: %s.", e.what()); @@ -45,10 +49,13 @@ LBRJointPositionCommandController::update(const rclcpp::Time & /*time*/, if (!lbr_joint_position_command || !(*lbr_joint_position_command)) { return controller_interface::return_type::OK; } - std::for_each(command_interfaces_.begin(), command_interfaces_.end(), - [lbr_joint_position_command, idx = 0](auto &command_interface) mutable { - command_interface.set_value((*lbr_joint_position_command)->joint_position[idx++]); - }); + std::for_each( + command_interfaces_.begin(), command_interfaces_.end(), + [lbr_joint_position_command, idx = 0](auto &command_interface) mutable { + command_interface.set_value( + (*lbr_joint_position_command) + ->joint_position[idx++]); // important to post-increment idx, don't use ++idx + }); return controller_interface::return_type::OK; } @@ -66,6 +73,20 @@ controller_interface::CallbackReturn LBRJointPositionCommandController::on_deact const rclcpp_lifecycle::State & /*previous_state*/) { return controller_interface::CallbackReturn::SUCCESS; } + +void LBRJointPositionCommandController::configure_joint_names_() { + if (joint_names_.size() != lbr_fri_ros2::N_JNTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + joint_names_.size(), lbr_fri_ros2::N_JNTS); + throw std::runtime_error("Failed to configure joint names."); + } + std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); + for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); + } +} } // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" diff --git a/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp index d858d220..cafd0a63 100644 --- a/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp +++ b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp @@ -21,13 +21,10 @@ controller_interface::CallbackReturn LBRStateBroadcaster::on_init() { rt_state_publisher_ptr_ = std::make_shared>( state_publisher_ptr_); - if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { - RCLCPP_ERROR( - this->get_node()->get_logger(), - "Number of joint names (%ld) does not match the number of joints in the robot (%d).", - joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - return controller_interface::CallbackReturn::ERROR; + if (!this->get_node()->has_parameter("robot_name")) { + this->get_node()->declare_parameter("robot_name", "lbr"); } + configure_joint_names_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize LBR state broadcaster with: %s.", e.what()); @@ -154,6 +151,20 @@ void LBRStateBroadcaster::init_state_msg_() { rt_state_publisher_ptr_->msg_.time_stamp_sec = std::numeric_limits::quiet_NaN(); rt_state_publisher_ptr_->msg_.tracking_performance = std::numeric_limits::quiet_NaN(); } + +void LBRStateBroadcaster::configure_joint_names_() { + if (joint_names_.size() != lbr_fri_ros2::N_JNTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + joint_names_.size(), lbr_fri_ros2::N_JNTS); + throw std::runtime_error("Failed to configure joint names."); + } + std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); + for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); + } +} } // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" diff --git a/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp index a4f674f6..62d027e4 100644 --- a/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp @@ -28,6 +28,10 @@ controller_interface::CallbackReturn LBRTorqueCommandController::on_init() { "command/torque", 1, [this](const lbr_fri_idl::msg::LBRTorqueCommand::SharedPtr msg) { rt_lbr_torque_command_ptr_.writeFromNonRT(msg); }); + if (!this->get_node()->has_parameter("robot_name")) { + this->get_node()->declare_parameter("robot_name", "lbr"); + } + configure_joint_names_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize LBR torque command controller with: %s.", e.what()); @@ -44,7 +48,7 @@ LBRTorqueCommandController::update(const rclcpp::Time & /*time*/, if (!lbr_torque_command || !(*lbr_torque_command)) { return controller_interface::return_type::OK; } - for (std::size_t idx = 0; idx < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++idx) { + for (std::size_t idx = 0; idx < lbr_fri_ros2::N_JNTS; ++idx) { joint_position_command_interfaces_[idx].get().set_value( (*lbr_torque_command)->joint_position[idx]); torque_command_interfaces_[idx].get().set_value((*lbr_torque_command)->torque[idx]); @@ -80,19 +84,19 @@ bool LBRTorqueCommandController::reference_command_interfaces_() { torque_command_interfaces_.emplace_back(std::ref(command_interface)); } } - if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (joint_position_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), "Number of joint position command interfaces '%ld' does not match the number of joints " "in the robot '%d'.", - joint_position_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + joint_position_command_interfaces_.size(), lbr_fri_ros2::N_JNTS); return false; } - if (torque_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (torque_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR(this->get_node()->get_logger(), "Number of torque command interfaces '%ld' does not match the number of joints " "in the robot '%d'.", - torque_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + torque_command_interfaces_.size(), lbr_fri_ros2::N_JNTS); return false; } return true; @@ -102,6 +106,20 @@ void LBRTorqueCommandController::clear_command_interfaces_() { joint_position_command_interfaces_.clear(); torque_command_interfaces_.clear(); } + +void LBRTorqueCommandController::configure_joint_names_() { + if (joint_names_.size() != lbr_fri_ros2::N_JNTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + joint_names_.size(), lbr_fri_ros2::N_JNTS); + throw std::runtime_error("Failed to configure joint names."); + } + std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); + for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); + } +} } // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" diff --git a/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp index 53445f1f..87e669e4 100644 --- a/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp @@ -33,6 +33,10 @@ controller_interface::CallbackReturn LBRWrenchCommandController::on_init() { "command/wrench", 1, [this](const lbr_fri_idl::msg::LBRWrenchCommand::SharedPtr msg) { rt_lbr_wrench_command_ptr_.writeFromNonRT(msg); }); + if (!this->get_node()->has_parameter("robot_name")) { + this->get_node()->declare_parameter("robot_name", "lbr"); + } + configure_joint_names_(); } catch (const std::exception &e) { RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize LBR wrench command controller with: %s.", e.what()); @@ -49,7 +53,7 @@ LBRWrenchCommandController::update(const rclcpp::Time & /*time*/, if (!lbr_wrench_command || !(*lbr_wrench_command)) { return controller_interface::return_type::OK; } - for (std::size_t idx = 0; idx < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++idx) { + for (std::size_t idx = 0; idx < lbr_fri_ros2::N_JNTS; ++idx) { joint_position_command_interfaces_[idx].get().set_value( (*lbr_wrench_command)->joint_position[idx]); } @@ -87,12 +91,12 @@ bool LBRWrenchCommandController::reference_command_interfaces_() { wrench_command_interfaces_.emplace_back(std::ref(command_interface)); } } - if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (joint_position_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), "Number of joint position command interfaces '%ld' does not match the number of joints " "in the robot '%d'.", - joint_position_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + joint_position_command_interfaces_.size(), lbr_fri_ros2::N_JNTS); return false; } if (wrench_command_interfaces_.size() != CARTESIAN_DOF) { @@ -108,6 +112,20 @@ void LBRWrenchCommandController::clear_command_interfaces_() { joint_position_command_interfaces_.clear(); wrench_command_interfaces_.clear(); } + +void LBRWrenchCommandController::configure_joint_names_() { + if (joint_names_.size() != lbr_fri_ros2::N_JNTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + joint_names_.size(), lbr_fri_ros2::N_JNTS); + throw std::runtime_error("Failed to configure joint names."); + } + std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); + for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); + } +} } // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp new file mode 100644 index 00000000..379e89f6 --- /dev/null +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -0,0 +1,246 @@ +#include "lbr_ros2_control/controllers/twist_controller.hpp" + +namespace lbr_ros2_control { +TwistController::TwistController() : rt_twist_ptr_(nullptr), twist_subscription_ptr_(nullptr) {} + +controller_interface::InterfaceConfiguration +TwistController::command_interface_configuration() const { + controller_interface::InterfaceConfiguration interface_configuration; + interface_configuration.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto &joint_name : joint_names_) { + interface_configuration.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION); + } + return interface_configuration; +} + +controller_interface::InterfaceConfiguration +TwistController::state_interface_configuration() const { + controller_interface::InterfaceConfiguration interface_configuration; + interface_configuration.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto &joint_name : joint_names_) { + interface_configuration.names.push_back(joint_name + "/" + hardware_interface::HW_IF_POSITION); + } + interface_configuration.names.push_back(std::string(HW_IF_AUXILIARY_PREFIX) + "/" + + HW_IF_SAMPLE_TIME); + interface_configuration.names.push_back(std::string(HW_IF_AUXILIARY_PREFIX) + "/" + + HW_IF_SESSION_STATE); + return interface_configuration; +} + +controller_interface::CallbackReturn TwistController::on_init() { + try { + twist_subscription_ptr_ = this->get_node()->create_subscription( + "command/twist", 1, [this](const geometry_msgs::msg::Twist::SharedPtr msg) { + rt_twist_ptr_.writeFromNonRT(msg); + updates_since_last_command_ = 0; + }); + if (!this->get_node()->has_parameter("robot_description")) { + this->get_node()->declare_parameter("robot_description", ""); + } + if (!this->get_node()->has_parameter("robot_name")) { + this->get_node()->declare_parameter("robot_name", "lbr"); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.chain_root")) { + this->get_node()->declare_parameter("inv_jac_ctrl.chain_root", "lbr_link_0"); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.chain_tip")) { + this->get_node()->declare_parameter("inv_jac_ctrl.chain_tip", "lbr_link_ee"); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.twist_in_tip_frame")) { + this->get_node()->declare_parameter("inv_jac_ctrl.twist_in_tip_frame", true); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.damping")) { + this->get_node()->declare_parameter("inv_jac_ctrl.damping", 0.2); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.max_linear_velocity")) { + this->get_node()->declare_parameter("inv_jac_ctrl.max_linear_velocity", 0.1); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.max_angular_velocity")) { + this->get_node()->declare_parameter("inv_jac_ctrl.max_angular_velocity", 0.1); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.joint_gains")) { + this->get_node()->declare_parameter("inv_jac_ctrl.joint_gains", + std::vector(lbr_fri_ros2::N_JNTS, 0.0)); + } + if (!this->get_node()->has_parameter("inv_jac_ctrl.cartesian_gains")) { + this->get_node()->declare_parameter("inv_jac_ctrl.cartesian_gains", + std::vector(lbr_fri_ros2::CARTESIAN_DOF, 0.0)); + } + if (!this->get_node()->has_parameter("timeout")) { + this->get_node()->declare_parameter("timeout", 0.2); + } + robot_description_ = this->get_node()->get_parameter("robot_description").as_string(); + if (robot_description_.empty()) { + throw std::runtime_error("No robot description provided"); + } + configure_joint_names_(); + configure_inv_jac_ctrl_impl_(); + log_info_(); + timeout_ = this->get_node()->get_parameter("timeout").as_double(); + } catch (const std::exception &e) { + RCLCPP_ERROR(this->get_node()->get_logger(), "Failed to initialize twist controller with: %s.", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type TwistController::update(const rclcpp::Time & /*time*/, + const rclcpp::Duration &period) { + auto twist_command = rt_twist_ptr_.readFromRT(); + if (!twist_command || !(*twist_command)) { + return controller_interface::return_type::OK; + } + if (!inv_jac_ctrl_impl_ptr_) { + RCLCPP_ERROR(this->get_node()->get_logger(), "Inverse Jacobian controller not initialized."); + return controller_interface::return_type::ERROR; + } + if (static_cast(session_state_interface_ptr_->get().get_value()) != + KUKA::FRI::ESessionState::COMMANDING_ACTIVE) { + return controller_interface::return_type::OK; + } + if (updates_since_last_command_ > static_cast(timeout_ / period.seconds())) { + RCLCPP_ERROR(this->get_node()->get_logger(), + "No twist command received within %.3f s. Stopping the controller.", timeout_); + return controller_interface::return_type::ERROR; + } + + // pass joint positions to q_ + std::for_each(q_.begin(), q_.end(), [&, i = 0](double &q_i) mutable { + q_i = this->state_interfaces_[i].get_value(); + ++i; + }); + + // compute the joint velocity from the twist command target + inv_jac_ctrl_impl_ptr_->compute(*twist_command, q_, dq_); + + // pass joint positions to hardware + std::for_each(q_.begin(), q_.end(), [&, i = 0](const double &q_i) mutable { + this->command_interfaces_[i].set_value( + q_i + dq_[i] * sample_time_state_interface_ptr_->get().get_value()); + ++i; + }); + + ++updates_since_last_command_; + + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn +TwistController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +TwistController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + if (!reference_state_interfaces_()) { + return controller_interface::CallbackReturn::ERROR; + } + reset_command_buffer_(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +TwistController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { + clear_state_interfaces_(); + reset_command_buffer_(); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool TwistController::reference_state_interfaces_() { + for (auto &state_interface : state_interfaces_) { + if (state_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { + joint_position_state_interfaces_.emplace_back(std::ref(state_interface)); + } + if (state_interface.get_interface_name() == HW_IF_SAMPLE_TIME) { + sample_time_state_interface_ptr_ = + std::make_unique>( + std::ref(state_interface)); + } + if (state_interface.get_interface_name() == HW_IF_SESSION_STATE) { + session_state_interface_ptr_ = + std::make_unique>( + std::ref(state_interface)); + } + } + if (joint_position_state_interfaces_.size() != lbr_fri_ros2::N_JNTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint position state interfaces '%ld' does not match the number of joints " + "in the robot '%d'.", + joint_position_state_interfaces_.size(), lbr_fri_ros2::N_JNTS); + return false; + } + return true; +} + +void TwistController::clear_state_interfaces_() { joint_position_state_interfaces_.clear(); } + +void TwistController::reset_command_buffer_() { + rt_twist_ptr_ = + realtime_tools::RealtimeBuffer>(nullptr); +}; + +void TwistController::configure_joint_names_() { + if (joint_names_.size() != lbr_fri_ros2::N_JNTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint names (%ld) does not match the number of joints in the robot (%d).", + joint_names_.size(), lbr_fri_ros2::N_JNTS); + throw std::runtime_error("Failed to configure joint names."); + } + std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); + for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); + } +} + +void TwistController::configure_inv_jac_ctrl_impl_() { + if (this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array().size() != + lbr_fri_ros2::N_JNTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint gains (%ld) does not match the number of joints in the robot (%d).", + this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array().size(), + lbr_fri_ros2::N_JNTS); + throw std::runtime_error("Failed to configure joint gains."); + } + if (this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array().size() != + lbr_fri_ros2::CARTESIAN_DOF) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of cartesian gains (%ld) does not match the number of cartesian degrees of freedom " + "(%d).", + this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array().size(), + lbr_fri_ros2::CARTESIAN_DOF); + throw std::runtime_error("Failed to configure cartesian gains."); + } + lbr_fri_ros2::jnt_array_t joint_gains_array; + for (unsigned int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + joint_gains_array[i] = + this->get_node()->get_parameter("inv_jac_ctrl.joint_gains").as_double_array()[i]; + } + lbr_fri_ros2::cart_array_t cartesian_gains_array; + for (unsigned int i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { + cartesian_gains_array[i] = + this->get_node()->get_parameter("inv_jac_ctrl.cartesian_gains").as_double_array()[i]; + } + inv_jac_ctrl_impl_ptr_ = std::make_unique( + robot_description_, + lbr_fri_ros2::InvJacCtrlParameters{ + this->get_node()->get_parameter("inv_jac_ctrl.chain_root").as_string(), + this->get_node()->get_parameter("inv_jac_ctrl.chain_tip").as_string(), + this->get_node()->get_parameter("inv_jac_ctrl.twist_in_tip_frame").as_bool(), + this->get_node()->get_parameter("inv_jac_ctrl.damping").as_double(), + this->get_node()->get_parameter("inv_jac_ctrl.max_linear_velocity").as_double(), + this->get_node()->get_parameter("inv_jac_ctrl.max_angular_velocity").as_double(), + joint_gains_array, cartesian_gains_array}); +} + +void TwistController::log_info_() const { inv_jac_ctrl_impl_ptr_->log_info(); } +} // namespace lbr_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(lbr_ros2_control::TwistController, controller_interface::ControllerInterface) diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 30753044..ebab71e4 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -11,21 +11,15 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { return ret; } - // parameters_ from config/lbr_system_interface.xacro + // parameters_ from lbr_system_interface.xacro (default configurations located in + // lbr_description/ros2_control/lbr_system_interface.xacro) if (!parse_parameters_(system_info)) { return controller_interface::CallbackReturn::ERROR; } // setup driver - lbr_fri_ros2::PIDParameters pid_parameters; lbr_fri_ros2::CommandGuardParameters command_guard_parameters; lbr_fri_ros2::StateInterfaceParameters state_interface_parameters; - pid_parameters.p = parameters_.pid_p; - pid_parameters.i = parameters_.pid_i; - pid_parameters.d = parameters_.pid_d; - pid_parameters.i_max = parameters_.pid_i_max; - pid_parameters.i_min = parameters_.pid_i_min; - pid_parameters.antiwindup = parameters_.pid_antiwindup; for (std::size_t idx = 0; idx < system_info.joints.size(); ++idx) { command_guard_parameters.joint_names[idx] = system_info.joints[idx].name; command_guard_parameters.max_positions[idx] = @@ -37,14 +31,12 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { command_guard_parameters.max_torques[idx] = std::stod(system_info.joints[idx].parameters.at("max_torque")); } - state_interface_parameters.external_torque_cutoff_frequency = - parameters_.external_torque_cutoff_frequency; - state_interface_parameters.measured_torque_cutoff_frequency = - parameters_.measured_torque_cutoff_frequency; + state_interface_parameters.external_torque_tau = parameters_.external_torque_tau; + state_interface_parameters.measured_torque_tau = parameters_.measured_torque_tau; try { async_client_ptr_ = std::make_shared( - parameters_.client_command_mode, pid_parameters, command_guard_parameters, + parameters_.client_command_mode, parameters_.joint_position_tau, command_guard_parameters, parameters_.command_guard_variant, state_interface_parameters, parameters_.open_loop); app_ptr_ = std::make_unique(async_client_ptr_); } catch (const std::exception &e) { @@ -60,6 +52,13 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { nan_last_hw_states_(); // setup force-torque estimator + std::transform(info_.sensors[1].parameters.at("enabled").begin(), + info_.sensors[1].parameters.at("enabled").end(), + info_.sensors[1].parameters.at("enabled").begin(), + ::tolower); // convert to lower case + ft_parameters_.enabled = info_.sensors[1].parameters.at("enabled") == "true"; + ft_parameters_.update_rate = std::stoul(info_.sensors[1].parameters.at("update_rate")); + ft_parameters_.rt_prio = std::stoi(info_.sensors[1].parameters.at("rt_prio")); ft_parameters_.chain_root = info_.sensors[1].parameters.at("chain_root"); ft_parameters_.chain_tip = info_.sensors[1].parameters.at("chain_tip"); ft_parameters_.damping = std::stod(info_.sensors[1].parameters.at("damping")); @@ -69,16 +68,20 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { ft_parameters_.torque_x_th = std::stod(info_.sensors[1].parameters.at("torque_x_th")); ft_parameters_.torque_y_th = std::stod(info_.sensors[1].parameters.at("torque_y_th")); ft_parameters_.torque_z_th = std::stod(info_.sensors[1].parameters.at("torque_z_th")); - ft_estimator_ptr_ = std::make_unique( - info_.original_xml, ft_parameters_.chain_root, ft_parameters_.chain_tip, - lbr_fri_ros2::FTEstimator::cart_array_t{ - ft_parameters_.force_x_th, - ft_parameters_.force_y_th, - ft_parameters_.force_z_th, - ft_parameters_.torque_x_th, - ft_parameters_.torque_y_th, - ft_parameters_.torque_z_th, - }); + if (ft_parameters_.enabled) { + ft_estimator_impl_ptr_ = std::make_shared( + info_.original_xml, ft_parameters_.chain_root, ft_parameters_.chain_tip, + lbr_fri_ros2::cart_array_t{ + ft_parameters_.force_x_th, + ft_parameters_.force_y_th, + ft_parameters_.force_z_th, + ft_parameters_.torque_x_th, + ft_parameters_.torque_y_th, + ft_parameters_.torque_z_th, + }); + ft_estimator_ptr_ = std::make_unique(ft_estimator_impl_ptr_, + ft_parameters_.update_rate); + } if (!verify_number_of_joints_()) { return controller_interface::CallbackReturn::ERROR; @@ -154,14 +157,16 @@ std::vector SystemInterface::export_state_in state_interfaces.emplace_back(auxiliary_sensor.name, HW_IF_TIME_STAMP_NANO_SEC, &hw_time_stamp_nano_sec_); - // additional force-torque state interface - const auto &estimated_ft_sensor = info_.sensors[1]; - state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_FORCE_X, &hw_ft_[0]); - state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_FORCE_Y, &hw_ft_[1]); - state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_FORCE_Z, &hw_ft_[2]); - state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_TORQUE_X, &hw_ft_[3]); - state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_TORQUE_Y, &hw_ft_[4]); - state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_TORQUE_Z, &hw_ft_[5]); + // additional force-torque state interface (if enabled) + if (ft_parameters_.enabled) { + const auto &estimated_ft_sensor = info_.sensors[1]; + state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_FORCE_X, &hw_ft_[0]); + state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_FORCE_Y, &hw_ft_[1]); + state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_FORCE_Z, &hw_ft_[2]); + state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_TORQUE_X, &hw_ft_[3]); + state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_TORQUE_Y, &hw_ft_[4]); + state_interfaces.emplace_back(estimated_ft_sensor.name, HW_IF_TORQUE_Z, &hw_ft_[5]); + } return state_interfaces; } @@ -249,6 +254,18 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l } std::this_thread::sleep_for(std::chrono::seconds(1)); } + + // ft sensor + if (!ft_estimator_ptr_ && ft_parameters_.enabled) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::ERROR + << "Failed to instantiate FTEstimator despite user request." + << lbr_fri_ros2::ColorScheme::ENDC); + return controller_interface::CallbackReturn::ERROR; + } + if (ft_estimator_ptr_) { + ft_estimator_ptr_->run_async(ft_parameters_.rt_prio); + } return controller_interface::CallbackReturn::SUCCESS; } @@ -256,6 +273,9 @@ controller_interface::CallbackReturn SystemInterface::on_deactivate(const rclcpp_lifecycle::State &) { app_ptr_->request_stop(); app_ptr_->close_udp_socket(); + if (ft_estimator_ptr_) { + ft_estimator_ptr_->request_stop(); + } return controller_interface::CallbackReturn::SUCCESS; } @@ -284,6 +304,7 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim << lbr_fri_ros2::ColorScheme::ENDC); app_ptr_->request_stop(); app_ptr_->close_udp_socket(); + ft_estimator_ptr_->request_stop(); return hardware_interface::return_type::ERROR; } @@ -304,8 +325,13 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim update_last_hw_states_(); // additional force-torque state interface - ft_estimator_ptr_->compute(hw_lbr_state_.measured_joint_position, hw_lbr_state_.external_torque, - hw_ft_, ft_parameters_.damping); + if (ft_parameters_.enabled) { + // note that (if enabled) the computation is performed asynchronously to not block the main + // thread + ft_estimator_impl_ptr_->set_q(hw_lbr_state_.measured_joint_position); + ft_estimator_impl_ptr_->set_tau_ext(hw_lbr_state_.external_torque); + ft_estimator_impl_ptr_->get_f_ext_tf(hw_ft_); + } return hardware_interface::return_type::OK; } @@ -370,22 +396,17 @@ bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo & parameters_.rt_prio = std::stoul(info_.hardware_parameters["rt_prio"]); std::transform(info_.hardware_parameters["open_loop"].begin(), info_.hardware_parameters["open_loop"].end(), - info_.hardware_parameters["open_loop"].begin(), ::tolower); + info_.hardware_parameters["open_loop"].begin(), + ::tolower); // convert to lower case parameters_.open_loop = info_.hardware_parameters["open_loop"] == "true"; std::transform(info_.hardware_parameters["pid_antiwindup"].begin(), info_.hardware_parameters["pid_antiwindup"].end(), - info_.hardware_parameters["pid_antiwindup"].begin(), ::tolower); - parameters_.pid_p = std::stod(info_.hardware_parameters["pid_p"]); - parameters_.pid_i = std::stod(info_.hardware_parameters["pid_i"]); - parameters_.pid_d = std::stod(info_.hardware_parameters["pid_d"]); - parameters_.pid_i_max = std::stod(info_.hardware_parameters["pid_i_max"]); - parameters_.pid_i_min = std::stod(info_.hardware_parameters["pid_i_min"]); - parameters_.pid_antiwindup = info_.hardware_parameters["pid_antiwindup"] == "true"; + info_.hardware_parameters["pid_antiwindup"].begin(), + ::tolower); // convert to lower case + parameters_.joint_position_tau = std::stod(info_.hardware_parameters["joint_position_tau"]); parameters_.command_guard_variant = system_info.hardware_parameters.at("command_guard_variant"); - parameters_.external_torque_cutoff_frequency = - std::stod(info_.hardware_parameters["external_torque_cutoff_frequency"]); - parameters_.measured_torque_cutoff_frequency = - std::stod(info_.hardware_parameters["measured_torque_cutoff_frequency"]); + parameters_.external_torque_tau = std::stod(info_.hardware_parameters["external_torque_tau"]); + parameters_.measured_torque_tau = std::stod(info_.hardware_parameters["measured_torque_tau"]); } catch (const std::out_of_range &e) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::ERROR @@ -435,12 +456,11 @@ void SystemInterface::nan_state_interfaces_() { } bool SystemInterface::verify_number_of_joints_() { - if (info_.joints.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (info_.joints.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::ERROR - << "Expected '" << KUKA::FRI::LBRState::NUMBER_OF_JOINTS - << "' joints in URDF, got '" << info_.joints.size() << "'" - << lbr_fri_ros2::ColorScheme::ENDC); + << "Expected '" << lbr_fri_ros2::N_JNTS << "' joints in URDF, got '" + << info_.joints.size() << "'" << lbr_fri_ros2::ColorScheme::ENDC); return false; } return true; @@ -524,14 +544,17 @@ bool SystemInterface::verify_sensors_() { if (!verify_auxiliary_sensor_()) { return false; } - if (!verify_estimated_ft_sensor_()) { - return false; + if (ft_parameters_.enabled) { + if (!verify_estimated_ft_sensor_()) { + return false; + } } return true; } bool SystemInterface::verify_auxiliary_sensor_() { - // check all interfaces are defined in config/lbr_system_interface.xacro + // check all interfaces are defined in lbr_system_interface.xacro (located in + // lbr_description/ros2_control/lbr_system_interface.xacro) const auto &auxiliary_sensor = info_.sensors[0]; if (auxiliary_sensor.name != HW_IF_AUXILIARY_PREFIX) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME),