diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 4e5c6cf7..d2b36d37 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,4 +1,2 @@ ## code changes will send PR to following users -* @VX792 -* @Svastits -* @kovacsge11 +* @Svastits @kovacsge11 diff --git a/doc/wiki/3_Sunrise_FRI.md b/doc/wiki/3_Sunrise_FRI.md index 19f8314f..a3c11b7c 100644 --- a/doc/wiki/3_Sunrise_FRI.md +++ b/doc/wiki/3_Sunrise_FRI.md @@ -49,7 +49,7 @@ This starts the 3 core components of every driver (described in the [Non-real-ti - `joint_group_impedance_controller` ([configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/joint_impedance_controller_config.yaml)) - `effort_controller` (of type `JointGroupEffortController`, [configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/effort_controller_config.yaml)) - [`control_mode_handler`](https://github.com/kroshu/kuka_controllers?tab=readme-ov-file#control_mode_handler) (no configuration file) - +- `external_torque_broadcaster` (of type `JointStateBroadcaster`, [configuration file](https://github.com/kroshu/kuka_drivers/tree/master/kuka_sunrise_fri_driver/config/external_torque_broadcaster_config.yaml), publishes a `JointState` message type on the topic `external_torque_broadcaster/joint_states` containing the measured external torques for every joint) 3. After successful startup, the `robot_manager` node has to be activated to start the cyclic communication with the robot controller (before this only a collapsed robot is visible in `rviz`): ``` ros2 lifecycle set robot_manager configure @@ -74,6 +74,7 @@ Both launch files support the following argument: - `jtc_config`: the location of the configuration file for the `joint_trajectory_controller` (defaults to `kuka_sunrise_fri_driver/config/joint_trajectory_controller_config.yaml`) - `jic_config`: the location of the configuration file for the `joint_impedance_controller` (defaults to `kuka_sunrise_fri_driver/config/joint_impedance_controller_config.yaml`) - `ec_config`: the location of the configuration file for the `effort_controller` (defaults to `kuka_sunrise_fri_driver/config/effort_controller_config.yaml`) +- `etb_config`: the location of the configuration file for the `external_torque_broadcaster` (defaults to `kuka_sunrise_fri_driver/config/external_torque_broadcaster._config.yaml`) The `startup_with_rviz.launch.py` additionally contains one argument: diff --git a/kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp b/kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp index 41a0bcf6..56999b14 100644 --- a/kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp +++ b/kuka_drivers_core/include/kuka_drivers_core/controller_names.hpp @@ -19,6 +19,8 @@ namespace kuka_drivers_core { /* Fixed controller names */ static constexpr char JOINT_STATE_BROADCASTER[] = "joint_state_broadcaster"; +static constexpr char EXTERNAL_TORQUE_BROADCASTER[] = "external_torque_broadcaster"; + static constexpr char CONTROL_MODE_HANDLER[] = "control_mode_handler"; static constexpr char EVENT_BROADCASTER[] = "event_broadcaster"; static constexpr char FRI_CONFIGURATION_CONTROLLER[] = "fri_configuration_controller"; diff --git a/kuka_sunrise_fri_driver/config/external_torque_broadcaster_config.yaml b/kuka_sunrise_fri_driver/config/external_torque_broadcaster_config.yaml new file mode 100644 index 00000000..5ffea8a8 --- /dev/null +++ b/kuka_sunrise_fri_driver/config/external_torque_broadcaster_config.yaml @@ -0,0 +1,15 @@ +external_torque_broadcaster: + ros__parameters: + interfaces: + - external_torque + joints: + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 + - joint_7 + map_interface_to_joint_state: + effort: external_torque + use_local_topics: true diff --git a/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml b/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml index d340c2b7..bb5edcb1 100755 --- a/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml +++ b/kuka_sunrise_fri_driver/config/ros2_controller_config.yaml @@ -12,6 +12,8 @@ controller_manager: type: kuka_controllers/FRIStateBroadcaster event_broadcaster: type: kuka_controllers/EventBroadcaster + external_torque_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster # Configuration controllers joint_group_impedance_controller: diff --git a/kuka_sunrise_fri_driver/launch/startup.launch.py b/kuka_sunrise_fri_driver/launch/startup.launch.py index 7ab660f0..913a89e1 100644 --- a/kuka_sunrise_fri_driver/launch/startup.launch.py +++ b/kuka_sunrise_fri_driver/launch/startup.launch.py @@ -39,6 +39,7 @@ def launch_setup(context, *args, **kwargs): jtc_config = LaunchConfiguration("jtc_config") jic_config = LaunchConfiguration("jic_config") ec_config = LaunchConfiguration("ec_config") + etb_config = LaunchConfiguration("etb_config") if ns.perform(context) == "": tf_prefix = "" else: @@ -116,6 +117,7 @@ def launch_setup(context, *args, **kwargs): jtc_config, jic_config, ec_config, + etb_config, { "hardware_components_initial_state": { "unconfigured": [tf_prefix + robot_model.perform(context)] @@ -159,6 +161,7 @@ def controller_spawner(controller_names, activate=False): controller_names = [ "joint_state_broadcaster", + "external_torque_broadcaster", "joint_trajectory_controller", "fri_configuration_controller", "fri_state_broadcaster", @@ -222,4 +225,11 @@ def generate_launch_description(): + "/config/effort_controller_config.yaml", ) ) + launch_arguments.append( + DeclareLaunchArgument( + "etb_config", + default_value=get_package_share_directory("kuka_sunrise_fri_driver") + + "/config/external_torque_broadcaster_config.yaml", + ) + ) return LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/kuka_sunrise_fri_driver/src/hardware_interface.cpp b/kuka_sunrise_fri_driver/src/hardware_interface.cpp index 4557e65a..dab061e0 100644 --- a/kuka_sunrise_fri_driver/src/hardware_interface.cpp +++ b/kuka_sunrise_fri_driver/src/hardware_interface.cpp @@ -468,7 +468,14 @@ void KukaFRIHardwareInterface::activateFrictionCompensation(double * values) con { for (int i = 0; i < DOF; i++) { - values[i] -= (values[i] / fabs(values[i]) * 0.1); + if (values[i] != 0.0) + { + values[i] -= (values[i] / fabs(values[i]) * 0.1); + } + else + { + values[i] -= 0.1; + } } } diff --git a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp index 85466a23..9c0fc64e 100644 --- a/kuka_sunrise_fri_driver/src/robot_manager_node.cpp +++ b/kuka_sunrise_fri_driver/src/robot_manager_node.cpp @@ -216,8 +216,8 @@ RobotManagerNode::on_activate(const rclcpp_lifecycle::State &) // Activate joint state broadcaster and controller for given control mode if (!kuka_drivers_core::changeControllerState( change_controller_state_client_, - {kuka_drivers_core::JOINT_STATE_BROADCASTER, kuka_drivers_core::FRI_STATE_BROADCASTER, - GetControllerName()}, + {kuka_drivers_core::JOINT_STATE_BROADCASTER, kuka_drivers_core::EXTERNAL_TORQUE_BROADCASTER, + kuka_drivers_core::FRI_STATE_BROADCASTER, GetControllerName()}, {kuka_drivers_core::JOINT_GROUP_IMPEDANCE_CONTROLLER})) { RCLCPP_ERROR(get_logger(), "Could not activate RT controllers"); @@ -245,7 +245,7 @@ RobotManagerNode::on_deactivate(const rclcpp_lifecycle::State &) if (!kuka_drivers_core::changeControllerState( change_controller_state_client_, {kuka_drivers_core::JOINT_GROUP_IMPEDANCE_CONTROLLER}, {GetControllerName(), kuka_drivers_core::JOINT_STATE_BROADCASTER, - kuka_drivers_core::FRI_STATE_BROADCASTER}, + kuka_drivers_core::EXTERNAL_TORQUE_BROADCASTER, kuka_drivers_core::FRI_STATE_BROADCASTER}, SwitchController::Request::BEST_EFFORT)) { RCLCPP_ERROR(get_logger(), "Could not deactivate RT controllers");