diff --git a/README.md b/README.md index 6a8dd0d3..08c5da0f 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). 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 7493620c..b0395b29 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: LBRROS2ControlMixin.arg_ctrl() ) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_ros2_control/config/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..d6ec6b9b 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'"] + ), ) ) diff --git a/lbr_bringup/launch/mock.launch.py b/lbr_bringup/launch/mock.launch.py index 3443431f..a87be2d4 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'"] + ), ) ) 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..5cc120ae 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=["chomp", "pilz_industrial_motion_planner", "stomp", "ompl"], + ) ) @staticmethod diff --git a/lbr_bringup/lbr_bringup/ros2_control.py b/lbr_bringup/lbr_bringup/ros2_control.py index a5a58627..e45a5fb9 100644 --- a/lbr_bringup/lbr_bringup/ros2_control.py +++ b/lbr_bringup/lbr_bringup/ros2_control.py @@ -35,9 +35,26 @@ def arg_ctrl() -> DeclareLaunchArgument: "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( @@ -122,10 +139,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 d6905e26..4cf72c0e 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -17,6 +17,9 @@ lbr_description lbr_fri_ros2 lbr_ros2_control + moveit_planners_chomp + moveit_planners_ompl + moveit_planners_stomp 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 index 7df54a6d..b356195d 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/config/admittance_control.yaml +++ b/lbr_demos/lbr_demos_advanced_cpp/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: [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] 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 eb08980a..f8426012 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 @@ -14,7 +14,7 @@ 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` + #. 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: @@ -55,7 +55,7 @@ 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` + #. 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/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 532ccd98..616751d0 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,7 +14,7 @@ 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` + #. 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,7 +54,7 @@ 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` + #. 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/setup.py b/lbr_demos/lbr_demos_advanced_py/setup.py index c465713a..c037589c 100644 --- a/lbr_demos/lbr_demos_advanced_py/setup.py +++ b/lbr_demos/lbr_demos_advanced_py/setup.py @@ -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 edf2a975..652f5aef 100644 --- a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst +++ b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst @@ -14,7 +14,7 @@ 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` + #. 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,7 +72,7 @@ Hardware ~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.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,7 +96,7 @@ 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` + #. 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,7 +134,7 @@ 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` + #. 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/doc/lbr_demos_py.rst b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst index aca3edce..2eeb8dd6 100644 --- a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst +++ b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst @@ -14,7 +14,7 @@ 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` + #. 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,7 +72,7 @@ Hardware ~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.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,7 +96,7 @@ 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` + #. 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,7 +134,7 @@ 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` + #. 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/setup.py b/lbr_demos/lbr_demos_py/setup.py index 76198e83..3fe2a962 100644 --- a/lbr_demos/lbr_demos_py/setup.py +++ b/lbr_demos/lbr_demos_py/setup.py @@ -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 42c17ea9..ccab9103 100644 --- a/lbr_demos/lbr_moveit/doc/lbr_moveit.rst +++ b/lbr_demos/lbr_moveit/doc/lbr_moveit.rst @@ -62,7 +62,7 @@ MoveIt Servo - Hardware ~~~~~~~~~~~~~~~~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.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,7 +122,7 @@ MoveIt via RViz - Hardware ~~~~~~~~~~~~~~~~~~~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.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/doc/lbr_moveit_cpp.rst b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst index 8442254b..0816829a 100644 --- a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst +++ b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst @@ -41,7 +41,7 @@ Hardware ~~~~~~~~ #. Client side configurations: - #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.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_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 174dc306..78a833fc 100644 --- a/lbr_description/gazebo/lbr_gazebo.xacro +++ b/lbr_description/gazebo/lbr_gazebo.xacro @@ -29,20 +29,20 @@ - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/lbr_ros2_control/config/lbr_system_parameters.yaml b/lbr_description/ros2_control/lbr_system_config.yaml similarity index 88% rename from lbr_ros2_control/config/lbr_system_parameters.yaml rename to lbr_description/ros2_control/lbr_system_config.yaml index 4ae4cbf2..6e110244 100644 --- a/lbr_ros2_control/config/lbr_system_parameters.yaml +++ b/lbr_description/ros2_control/lbr_system_config.yaml @@ -19,8 +19,11 @@ hardware: 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_ros2_control/config/lbr_system_interface.xacro b/lbr_description/ros2_control/lbr_system_interface.xacro similarity index 64% rename from lbr_ros2_control/config/lbr_system_interface.xacro rename to lbr_description/ros2_control/lbr_system_interface.xacro index 833af578..5a31f0af 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_description/ros2_control/lbr_system_interface.xacro @@ -1,10 +1,10 @@ + params="robot_name mode joint_limits system_config_path"> - + @@ -21,22 +21,22 @@ 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']['pid_p']} + ${system_config['hardware']['pid_i']} + ${system_config['hardware']['pid_d']} + ${system_config['hardware']['pid_i_max']} + ${system_config['hardware']['pid_i_min']} + ${system_config['hardware']['pid_antiwindup']} + ${system_config['hardware']['command_guard_variant']} + ${system_config['hardware']['external_torque_cutoff_frequency']} + ${system_config['hardware']['measured_torque_cutoff_frequency']} + ${system_config['hardware']['open_loop']} @@ -61,21 +61,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 +119,8 @@ ${max_position} ${max_velocity} ${max_torque} - + @@ -124,43 +130,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..fde51896 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 @@ - + - - + + - + @@ -285,12 +286,12 @@ - - + + - + \ 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..e9bfaa1e 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 @@ - + - - + + - + @@ -286,12 +287,12 @@ - - + + - + \ 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..6a5e5a27 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 @@ - + - - + + - + @@ -286,12 +287,12 @@ - - + + - + \ 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..1b9998f8 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 @@ - + - - + + - + @@ -286,12 +287,12 @@ - - + + - + \ No newline at end of file diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt index 47a55f94..04dff3fd 100644 --- a/lbr_fri_ros2/CMakeLists.txt +++ b/lbr_fri_ros2/CMakeLists.txt @@ -35,6 +35,8 @@ add_library(lbr_fri_ros2 src/command_guard.cpp src/filters.cpp src/ft_estimator.cpp + src/kinematics.cpp + src/worker.cpp ) target_include_directories(lbr_fri_ros2 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/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/filters.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp index f14d5915..6f62bdd6 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp @@ -12,6 +12,7 @@ #include "friLBRClient.h" #include "lbr_fri_idl/msg/lbr_state.hpp" +#include "lbr_fri_ros2/types.hpp" namespace lbr_fri_ros2 { class ExponentialFilter { @@ -96,12 +97,10 @@ class ExponentialFilter { }; class JointExponentialFilterArray { - using value_array_t = std::array; - public: JointExponentialFilterArray() = default; - void compute(const double *const current, value_array_t &previous); + void compute(const double *const current, jnt_array_t_ref previous); void initialize(const double &cutoff_frequency, const double &sample_time); inline const bool &is_initialized() const { return initialized_; }; @@ -122,17 +121,16 @@ struct PIDParameters { class JointPIDArray { protected: static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::JointPIDArray"; - using value_array_t = std::array; - using pid_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 compute(const_jnt_array_t_ref command_target, const_jnt_array_t_ref state, + const std::chrono::nanoseconds &dt, jnt_array_t_ref command); + void compute(const_jnt_array_t_ref command_target, const double *state, + const std::chrono::nanoseconds &dt, jnt_array_t_ref command); void log_info() const; protected: 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..bb46d0ae 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,28 +10,18 @@ #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, 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..23457637 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp @@ -7,10 +7,10 @@ #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 { @@ -22,17 +22,6 @@ 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}); @@ -40,7 +29,7 @@ class StateInterface { 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/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/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/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/filters.cpp b/lbr_fri_ros2/src/filters.cpp index 4047e59a..eb842ccc 100644 --- a/lbr_fri_ros2/src/filters.cpp +++ b/lbr_fri_ros2/src/filters.cpp @@ -29,12 +29,11 @@ double ExponentialFilter::compute_alpha_(const double &cutoff_frequency, 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; - }); +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; + }); } void JointExponentialFilterArray::initialize(const double &cutoff_frequency, @@ -54,16 +53,16 @@ JointPIDArray::JointPIDArray(const PIDParameters &pid_parameters) }); } -void JointPIDArray::compute(const value_array_t &command_target, const value_array_t &state, - const std::chrono::nanoseconds &dt, value_array_t &command) { +void JointPIDArray::compute(const_jnt_array_t_ref command_target, const_jnt_array_t_ref state, + const std::chrono::nanoseconds &dt, jnt_array_t_ref 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 JointPIDArray::compute(const value_array_t &command_target, const double *state, - const std::chrono::nanoseconds &dt, value_array_t &command) { +void JointPIDArray::compute(const_jnt_array_t_ref command_target, const double *state, + const std::chrono::nanoseconds &dt, jnt_array_t_ref 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; 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/state.cpp b/lbr_fri_ros2/src/interfaces/state.cpp index 7e49c9bd..3054bf55 100644 --- a/lbr_fri_ros2/src/interfaces/state.cpp +++ b/lbr_fri_ros2/src/interfaces/state.cpp @@ -16,8 +16,8 @@ void StateInterface::set_state(const_fri_state_t_ref state) { 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); } @@ -41,7 +41,7 @@ void StateInterface::set_state(const_fri_state_t_ref state) { }; 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(), @@ -53,8 +53,8 @@ void StateInterface::set_state_open_loop(const_fri_state_t_ref state, 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); } 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_position_command.cpp b/lbr_fri_ros2/test/test_position_command.cpp index 22cd7348..e2f7830d 100644 --- a/lbr_fri_ros2/test/test_position_command.cpp +++ b/lbr_fri_ros2/test/test_position_command.cpp @@ -24,7 +24,8 @@ int main() { 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.}; diff --git a/lbr_fri_ros2/test/test_torque_command.cpp b/lbr_fri_ros2/test/test_torque_command.cpp index 2af7f93a..9c580828 100644 --- a/lbr_fri_ros2/test/test_torque_command.cpp +++ b/lbr_fri_ros2/test/test_torque_command.cpp @@ -24,7 +24,8 @@ int main() { 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.}; diff --git a/lbr_fri_ros2/test/test_wrench_command.cpp b/lbr_fri_ros2/test/test_wrench_command.cpp index de88e0e6..9ad8b315 100644 --- a/lbr_fri_ros2/test/test_wrench_command.cpp +++ b/lbr_fri_ros2/test/test_wrench_command.cpp @@ -24,7 +24,8 @@ int main() { 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.}; 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..cf019396 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) @@ -33,6 +35,7 @@ add_library( 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 +50,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 +79,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 diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index c7939d25..64c91616 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -2,7 +2,7 @@ # managers regardless of their namespace. Usefull in multi-robot setups. /**/controller_manager: ros__parameters: - update_rate: 200 + update_rate: 100 # ROS 2 control broadcasters joint_state_broadcaster: @@ -23,6 +23,9 @@ 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 @@ -32,25 +35,31 @@ lbr_wrench_command_controller: type: lbr_ros2_control/LBRWrenchCommandController - # Admittance controller - admittance_controller: - type: lbr_ros2_control/AdmittanceController + 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 + 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: - - A1 - - A2 - - A3 - - A4 - - A5 - - A6 - - A7 + - lbr_A1 + - lbr_A2 + - lbr_A3 + - lbr_A4 + - lbr_A5 + - lbr_A6 + - lbr_A7 command_interfaces: - position state_interfaces: @@ -62,11 +71,34 @@ /**/forward_position_controller: ros__parameters: joints: - - A1 - - A2 - - A3 - - A4 - - A5 - - A6 - - A7 + - 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 + +/**/twist_controller: + ros__parameters: + robot_name: lbr + 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: 0.1 # maximum linear velocity + max_angular_velocity: 0.1 # maximum linear acceleration + timeout: 0.2 # stop controller if no command is received within this time [s] 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 0354f68a..07bc8c7c 100644 --- a/lbr_ros2_control/doc/lbr_ros2_control.rst +++ b/lbr_ros2_control/doc/lbr_ros2_control.rst @@ -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..f6909e11 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 @@ -14,12 +14,13 @@ #include "rclcpp/rclcpp.hpp" #include "friLBRState.h" +#include "lbr_fri_ros2/types.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" namespace lbr_ros2_control { class Admittance { - -} + // implement an addmittance... +}; class AdmittanceController : public controller_interface::ControllerInterface { static constexpr uint8_t CARTESIAN_DOF = 6; @@ -50,9 +51,9 @@ class AdmittanceController : public controller_interface::ControllerInterface { bool reference_state_interfaces_(); void clear_command_interfaces_(); void clear_state_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_; 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..a10a2ba0 --- /dev/null +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp @@ -0,0 +1,104 @@ +#ifndef LBR_ROS2_CONTROL__TWIST_CONTROLLER_HPP_ +#define LBR_ROS2_CONTROL__TWIST_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "eigen3/Eigen/Core" +#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/kinematics.hpp" +#include "lbr_fri_ros2/pinv.hpp" +#include "lbr_fri_ros2/types.hpp" +#include "lbr_ros2_control/system_interface_type_values.hpp" + +namespace lbr_ros2_control { +struct TwistParameters { + std::string chain_root; + std::string chain_tip; + double damping; + double max_linear_velocity; + double max_angular_velocity; +}; + +class TwistImpl { +public: + TwistImpl(const std::string &robot_description, const TwistParameters ¶meters); + + void compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target, + lbr_fri_ros2::const_jnt_array_t_ref q, lbr_fri_ros2::jnt_array_t_ref dq); + +protected: + TwistParameters parameters_; + + lbr_fri_ros2::jnt_array_t q_; + std::unique_ptr kinematics_ptr_; + Eigen::Matrix jacobian_inv_; + Eigen::Matrix twist_target_; +}; + +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_twist_impl_(); + + // some safety checks + std::atomic updates_since_last_command_ = 0; + double timeout_ = 0.2; + + // joint veloctiy computation + std::unique_ptr twist_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_; + std::unique_ptr> + session_state_interface_; + + // 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..9a43948b 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -27,6 +27,7 @@ #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 { @@ -55,8 +56,11 @@ struct SystemInterfaceParameters { }; 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 +164,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..932c667c 100644 --- a/lbr_ros2_control/package.xml +++ b/lbr_ros2_control/package.xml @@ -8,18 +8,25 @@ 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..f7b61c29 100644 --- a/lbr_ros2_control/src/controllers/admittance_controller.cpp +++ b/lbr_ros2_control/src/controllers/admittance_controller.cpp @@ -4,11 +4,12 @@ namespace lbr_ros2_control { AdmittanceController::AdmittanceController() {} controller_interface::InterfaceConfiguration -AdmittanceController::command_interface_configuration() { +AdmittanceController::command_interface_configuration() const { // reference joint position command interface } -controller_interface::InterfaceConfiguration AdmittanceController::state_interface_configuration() { +controller_interface::InterfaceConfiguration +AdmittanceController::state_interface_configuration() const { // retrieve estimated ft state interface } @@ -49,12 +50,12 @@ bool AdmittanceController::reference_command_interfaces_() { joint_position_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; } } @@ -74,4 +75,18 @@ void AdmittanceController::clear_command_interfaces_() { void AdmittanceController::clear_state_interfaces_() { estimated_ft_sensor_state_interface_.clear(); } + +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); + } +} } // namespace lbr_ros2_control 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..232acf93 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,8 @@ controller_interface::CallbackReturn LBRJointPositionCommandController::on_init( [this](const lbr_fri_idl::msg::LBRJointPositionCommand::SharedPtr msg) { rt_lbr_joint_position_command_ptr_.writeFromNonRT(msg); }); + 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()); @@ -47,7 +49,7 @@ LBRJointPositionCommandController::update(const rclcpp::Time & /*time*/, } 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++]); + command_interface.set_value((*lbr_joint_position_command)->joint_position[++idx]); }); return controller_interface::return_type::OK; } @@ -66,6 +68,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..765bdd5b 100644 --- a/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp +++ b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp @@ -21,13 +21,8 @@ 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; - } + 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 +149,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..b56bd062 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,8 @@ 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); }); + 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 +46,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 +82,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 +104,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..37a278d9 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,8 @@ 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); }); + 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 +51,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 +89,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 +110,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..0483180c --- /dev/null +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -0,0 +1,212 @@ +#include "lbr_ros2_control/controllers/twist_controller.hpp" + +namespace lbr_ros2_control { +TwistImpl::TwistImpl(const std::string &robot_description, const TwistParameters ¶meters) + : parameters_(parameters) { + kinematics_ptr_ = std::make_unique( + robot_description, parameters_.chain_root, parameters_.chain_tip); +} + +void TwistImpl::compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target, + lbr_fri_ros2::const_jnt_array_t_ref q, lbr_fri_ros2::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; + + // 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); + }); + + // compute jacobian + auto jacobian = kinematics_ptr_->compute_jacobian(q); + jacobian_inv_ = lbr_fri_ros2::pinv(jacobian.data, parameters_.damping); + + // compute target joint veloctiy and map it to dq + Eigen::Map>(dq.data()) = + jacobian_inv_ * twist_target_; +} + +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; + }); + this->get_node()->declare_parameter("robot_name", "lbr"); + this->get_node()->declare_parameter("chain_root", "lbr_link_0"); + this->get_node()->declare_parameter("chain_tip", "lbr_link_ee"); + this->get_node()->declare_parameter("damping", 0.2); + this->get_node()->declare_parameter("max_linear_velocity", 0.1); + this->get_node()->declare_parameter("max_angular_velocity", 0.1); + this->get_node()->declare_parameter("timeout", 0.2); + configure_joint_names_(); + configure_twist_impl_(); + 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 (!twist_impl_ptr_) { + RCLCPP_ERROR(this->get_node()->get_logger(), "Twist controller not initialized."); + return controller_interface::return_type::ERROR; + } + if (static_cast(session_state_interface_->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 + twist_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_->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_ = + std::make_unique>( + std::ref(state_interface)); + } + if (state_interface.get_interface_name() == HW_IF_SESSION_STATE) { + session_state_interface_ = + 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_twist_impl_() { + twist_impl_ptr_ = std::make_unique( + this->get_robot_description(), + TwistParameters{this->get_node()->get_parameter("chain_root").as_string(), + this->get_node()->get_parameter("chain_tip").as_string(), + this->get_node()->get_parameter("damping").as_double(), + this->get_node()->get_parameter("max_linear_velocity").as_double(), + this->get_node()->get_parameter("max_angular_velocity").as_double()}); +} +} // 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..91cbf710 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -11,7 +11,8 @@ 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; } @@ -60,6 +61,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 +77,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 +166,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 +263,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 +282,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 +313,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 +334,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,11 +405,13 @@ 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); + info_.hardware_parameters["pid_antiwindup"].begin(), + ::tolower); // convert to lower case 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"]); @@ -435,12 +472,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 +560,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),